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ABSTRACT 


The  role  of  the  Maneuvering  Target  Statistical  Tracker  (MTST),  a  Kalman  filter  tracking 
algorithm  based  on  the  Integrated  Omstein-Uhlenbeck  (lOU)  motion  process,  in  the 
Antisubmarine  Warfare  System  Evaluation  Tool  (ASSET)  is  examined  and  its  operation 
described.  ASSET  is  a  campaign  simulation  which  models  open-ocean  ASW  scenarios  featuring 
prosecution  of  hostile  submarines  by  friendly  submarines  and  aircraft  based  on  cues  provided 
by  data  fusion  centers.  The  heart  of  each  data  fusion  center  is  an  MTST  which  integrates  new 
contact  information  into  tracks.  Comparing  the  level  of  sophistication  of  the  tracking  algorithm 
to  that  of  the  contact  data  provided  to  it,  a  number  of  simplifications  are  proposed.  These 
include  using  reduced  complexity  lOU  prediction  and  Kalman  filter  equations;  the  use  of  pre- 
processed  variance  data  together  with  the  true  position  of  targets  to  estimate,  rather  than 
explicitly  calculate,  updated  track  states;  and  limiting  contact  processing  based  on  information 
content.  Results  indicate  a  good  simulation  of  tracker  output  is  produced  using  a  greatly 
simplified  algorithm.  This  technique  can  be  generalized  to  other  types  of  simulations  involving 
target  tracking. 
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I.  AN  INTRODUCTION  TO  ASSET 


The  Antisubmarine  Warfare  Systems  Evaluation  Tool  (ASSET)  is  a  campaign-level 
simulation  which  models  open  ocean  scenarios  involving  submarines,  maritime  patrol 
aircraft  (MPA),  shore-based  command  and  data-fusion  centers  and  a  wide  variety  of 
passive  acoustic  sensors.  Active  acoustic  and  non-acoustic  sensors  are  only  modeled 
using  a  simple  area  sensor  model  which  has  a  fixed  probability  of  detection  and  false 
alarm  rate.  The  simulation  scenario  is  specified  by  a  user-supplied  "architecture"  which 
determines  all  facets  of  environment,  command  control,  sensor  interaction  and  platform 
maneuver.  This  structure  is  input  to  a  desktop  computer/ workstation  (the  Apple 
Macintosh  n  in  this  version)  through  a  series  of  user-friendly  windows,  each  dealing  with 
a  specific  topic.  A  particular  configuration  of  sensors,  data  fusion  centers, 
communication  nodes  and  tactical  platforms  interact  in  a  particular  geographic  location 
against  an  analogous  enemy  force  structure. 

The  scenario  is  repeated  as  a  Monte  Carlo  simulation  to  produce  statistically 
meaningful  measures  of  effectiveness  (MOE’s).  Output  data  regarding  the  detection, 
localization  and  prosecution  of  enemy  submarines  can  then  provide  a  quantitative  basis 
for  decisions  regarding  ASW  Master  Plans,  Top  Level  Warfare  Requirements,  and  a 
variety  of  emerging  technology  assessments  and  system  appraisals  [Ref.  l:p.  iv].  These 
results  can  also  be  useful  in  conducting  quantitative  experimentation  with  regional  force 
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levels,  force  compositions  and  commitment  strategies;  command,  control, 
communications  and  intelligence  (C^I)  networks;  implications  of  foreign  technology 
advances;  and  fleet  exercise  planning.  The  current  version  of  ASSET  (version  1 .0)  limits 
the  scope  of  these  scenarios  to  open  ocean  search  and  prosecution  of  hostile  submarines 
by  cueing  friendly  submarines  and  MPA  with  wide-area  sensors,  and  modeling  the 
supporting  C^I  networks.  The  modular  nature  of  the  object-oriented  structure  of  ASSET 
makes  expanding  the  scope  of  the  simulation  possible  [Ref.  2:p.  1-2].  As  the 
construction  and  operating  costs  of  naval  platforms  spiral  upwards,  a  simulation  tool  of 
this  kind  will  be  increasingly  important  to  intelligently  manage  resources  vis-a-vis  a 
rapidly  evolving  spectrum  of  possible  threats. 

A.  OBJECTIVES  AND  RATIONALE 

The  purpose  of  this  thesis  is  three-fold; 

•  to  provide  a  description  of  the  target  tracking  algorithm  used  by  ASSET  and 
how  it  relates  to; 

1.  real-world  tracking. 

2.  the  ASSET  simulation  as  a  whole. 

•  to  examine  the  mathematical  development  of  the  tracking  algorithm. 

•  to  examine  possible  modifications  to  the  existing  Kalman  filter  tracking 
algorithm  to  better  match  the  data  input  to  it  and  increase  computational 
efficiency. 
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The  use  of  full  Kalman  filter  equations  for  elliptical  areas  of  uncertainty 
(AOU’s)  is  not  necessary  given  the  circular  AOU’s  that  ASSET’S  sensor  model 
produces.  Making  use  of  the  fact  that  the  computer  knows  the  ground  truth  locations 
of  the  platforms,  and  the  limited  data  input  to  the  filter,  it  will  be  shown  that  it  is 
possible  to  approximate  appropriately  distributed  filtered  state  positions  using 
simplified  forms  of  the  Kalman  filter  matrix  computations.  The  situations  when  the 
tracker  algorithm  is  used  can  also  be  limited  based  on  the  information  content  of  the 
contact.  Together,  these  modifications  yield  a  significant  reduction  in  the 
computational  complexity  of  the  tracking  algorithm,  which  is  the  principle  goal  of  this 
thesis. 

The  automatic  correlator  tracker  (ACT)  implemented  in  ASSET  is  a  stand-alone 
module  based  on  the  Ocean  Surveillance  Information  System  (OSIS)  baseline  upgrade 
single  hypothesis,  multiple  target,  Kalman  filter-based,  correlation  and  tracking 
algorithm  [Ref.  3:p.  4].  This  set  of  routines  contains  a  complete  representation  of  the 
OSIS  baseline  upgrade  automatic  correlator  tracker’s  (OBU-ACT)  functions,  with  the 
exception  of  LINK,  the  utility  that  evaluates  track  sets  which  are  potentially  legs  of  a 
single  target’s  track;  and  EQUATE,  which  makes  track  associations  for  contacts  pre¬ 
correlated  to  a  particular  platform  based  on  acoustic  or  electromagnetic  signature. 

The  OBU-ACT  package  is  designed  to  process  contact  information  derived  from 
position-only,  bearing-only,  and  position  and  velocity  reports.  The  covariance 
matrices  associated  with  these  reports  can  be  interpreted  as  an  elliptical  representation 
of  the  error  in  position  and  velocity.  The  capability  to  process  line  of  bearing 
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contacts  is  not  available  in  ASSET  (1.0).  The  method  used  to  model  sensor  systems 
in  ASSET  1.0  approximates  the  elliptical  errors  with  circular  ones.  This  reduces  the 
complexity  of  the  covariance  matrices  considerably.  Taking  advantage  of  the  large 
number  of  zero  elements  and  repeated  values  in  the  matrices  actually  constructed  by 
ASSET,  the  matrix  equations  invoh’ed  in  the  tracking  algorithm  can  be  reduced  to 
equivalent  scalar  ones  by  eliminating  all  the  zero-  multiplied  terms. 

Central  to  this  algorithm  is  the  Integrated  Omstein-Uhlenbeck  (lOU)  process 
which  models  target  motion  in  the  Maneuvering  Target  Statistical  Tracker  (MTST). 
The  MTST  uses  this  model,  together  with  contact  data  corrupted  by  noise,  to  estimate 
the  size  and  posibon  of  the  Submarine  Probability  Area  (SPA)  which  has  an  86 
percent  probability  of  containing  the  target’s  true  position.  The  major  modification  to 
the  algorithm  proposed  is  based  on  assuming  that  the  lOU  prediction  position 
distribution,  in  like  fashion  to  the  contact  position  distribution,  is  centered  on  the 
target’s  true  position.  This  results  in  a  distribution  of  SPA  centers  that  is  centered  c  . 
the  targets  true  position  also.  Thus,  only  the  SPA  variance  is  calculated,  not  its 
center  position,  which  is  drawn  randomly  from  the  estimated  distribution  about  the 
true  target  position.  Since  contact  data  does  not  play  a  role  in  computing  these 
variances,  they  may  be  calculated  prior  to  the  start  of  the  simulation.  A  table  of 
variances  for  the  sensor  AOU  val  aes  defined  for  the  scenario  and  an  appropriate 
range  of  contact  report  interarrival  times  can  be  constructed  and  referred  to  as  needed 
during  simulation  execution  to  minimize  the  time  spent  processing  contact  data. 
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B.  FUNDAMENTALS  OF  OPERATION. 


The  basic  structure  of  ASSET  was  adapted  from  COAST,  the  Common  LISP 
Architectural  Study  Tool.  Common  LISP  is  an  object-oriented  programming  language 
used  for  rapid  prototyping  and  artificial  intelligence  work.  A  general  overview  of  this 
structure  is  presented  to  facilitate  understanding  of  how  the  COAST/ASSET  system  is 
organized  and  the  critical  role  of  the  tracker  to  simulation  operation. 

1.  Objects  and  Object  Interactions. 

Object-oriented  programs  combine  data  with  associated  procedures  to  form  a 
hierarchical  network  of  self-contained  modules  known  as  objects.  These  objects  can 
then  be  invoked  by  each  other  according  to  the  program  methodology.  Program 
objects  are  independent  and  can  be  modified  without  affecting  other  objects  they 
communicate  with.  The  data  that  is  communicated  will  change,  but  the  relationship 
between  the  objects  does  not  necessarily  have  to.  Data  can  be  evaluated  internally  by 
an  object  without  affecting  the  analogous  data  in  a  separate  object. 

The  principle  of  instancing  allows  a  single  object  definition  to  create  any 
number  of  structures  within  the  program,  all  obeying  the  same  definition.  This 
allows  a  single  definition  of  a  submarine  object,  for  example,  to  be  given  many 
different  sets  of  parameters  each  representing  a  different  class  of  submarine.  Other 
objects  may  be  designated  to  impart  their  characteristics  to  the  new  submarine  object. 
This  process  of  inheritance  allows  the  properties  of  another  object,  an  acoustic  sensor 
object  for  example,  to  be  created  separately  from  the  submarine  object.  The 
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submarine  object  can  then  be  designated  to  be  a  kind  of  acoustic  sensor  and  it  will 
inherit  the  properties  of  the  acoustic  sensor  object  in  addition  to  its  own.  Another 
object  such  as  a  SURTASS  towed-array  surveillance  ship  object  can  also  inherit  the 
properties  of  the  acoustic  sensor  object,  with  different  parameters,  without  affecting 
the  parameters  of  the  acoustic  sensor  inherited  by  the  submarine  object. 

Complex  classes  of  platforms  can  be  formed  by  multiple  inheritance  of  the 
properties  of  basic  building  block  objects.  These  are  then  used  to  simulate  any 
number  of  platforms  of  that  class,  each  operating  independently  within  the  simulation. 
The  computer  memory  available  to  store  each  of  these  instances  of  the  object  becomes 
the  limiting  factor  to  the  complexity  of  the  scenario  to  be  examined.  The  organization 
of  objects  in  ASSET  consist  of  several  major  groups: 


•  Graphical  environment  objects  which  implement  the  standard  Macintosh 
graphical  user  interface  to  provide  input/output  by  means  of  a  mouse  based 
"point  and  click"  metaphor  featuring  pull-down  menus,dialog  boxes  with  labeled 
data  regions  and  radio  buttons,  and  windowed  presentation  of  multiple 
information  sources  simultaneously. 

•  Region  management  objects  which  construct  and  manage  distinct  regions  of 
varying  environmental  properties,  command  responsibility  or  platform  patrol 
assignment. 

•  Event  management  objects  which  queue  all  simulation  events  in  time  order 
sequence  and  parcel  them  out  to  the  appropriate  resolution  objects. 

•  Command  objects  which  perform  resource  allocation,  assigning  available  assets 
to  contact  cues  based  on  either  time  to  station  (MPA)  or  area  of  uncertainty  size 
(submarines). 

•  Automatic  Correlator  Tracker  (ACT)  objects  which  manage  the  grouping  and 
processing  of  contact  reports  into  target  tracks  creating  a  tactical  picture 
consisting  of  combinations  of  true  and  false  contacts. 
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•  Tactical  platform  objects  which  simulate  the  behavior  of  their  real  world 
counterparts.  These  are  dynamic  associations  of  component  objects  which  may 
change  in  response  to  simulation  events.  A  submarine  inheriting  the  properties 
of  a  Patroller  object  may  switch  to  an  Interceptor  after  making  a  detection. 

The  program  flow  from  object  to  object  is  not  sequential,  but  event  driven. 

Once  started,  the  simulation  clock  advances  time  relative  to  the  simulation.  Sensor 
objects  begin  glimpsing  and  motion  platforms  begin  moving.  Possible  detection 
events  are  sent  to  the  event  manager  for  proper  sequencing  and  resolution.  Detections 
are  communicated  up  the  designated  chain  of  command  to  data-fusion  centers  where 
an  instance  of  the  ACT  processes  them  and  integrates  them  into  the  tactical  picture. 
This  may  cause  a  command  object  to  direct  (or  redirect)  assets  in  response.  This 
process  continues  until  the  allotted  time  for  the  scenario  expires.  The  MOE  statistics 
for  that  run  are  added  to  the  MOE  file  and  the  next  iteration  of  the  scenario 
commences.  When  the  desired  number  of  iterations  are  complete  the  compiled 
summary  of  MOE  statistics  is  analyzed  to  interpret  the  results  of  the  simulation. 

2.  ASW  System  Architecture  Evaluation. 

In  order  to  evaluate  a  desired  architecture  it  must  be  conceptualized  in  great 
detail.  The  classic  computer  maxim  "garbage  in,  garbage  out"  is  especially  true  of 
ASSET  where  a  single  bad  parameter  value  can  render  the  results  meaningless. 

While  the  user-friendly  interface  facilitates  the  mechanical  process  of  inputing  data, 
the  abstract  nature  of  the  required  parameters  make  it  necessary  to  assume  values  of 
dubious  validity  at  times.  General  areas  which  require  quantified  data  include: 
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•  Communication  connectivities  which  represent  how  detection  reports  are  passed 
from  object  to  object  and  what  delays  are  involved  at  each  node. 

•  Command  organization  including  geographic  regions  of  responsibility. 

•  Environmental  data  for  acoustic  propagation  loss  as  a  function  of  range  and 
frequency  as  well  as  ambient  acoustic  noise  in  each  distinct  environmental 
region. 

•  Motion  plans  representing  the  operating  areas  and  interconnecting  tracks  which 
will  govern  tactical  platform  movement. 

•  Umpire  parameters  such  as  the  kill  probabilities  for  a  given  submarine  class 
against  each  possible  target  class  both  for  the  case  where  it  detects  the  enemy 
first  and  the  case  where  it  is  detected  by  the  enemy  first. 


The  individual  platforms  require  complex  definition  as  well.  A  submarine 
object,  for  example,  requires  parameters  for: 


•  breakoff  speed  between  "fast"  and  "slow"  acoustic  behavior. 

•  self-noise  at  slow  and  fast  speed. 

•  directivity  index  and  recognition  differential  (for  Sonar  Equation  computations). 

•  signature  frequency  emitted  and  intensity  at  slow  and  fast  speed. 

•  movement  speeds  when  patrolling  and  intercepting  and  detailed  motion  plan  to 
be  followed. 

•  weapons  loadout  and  level  at  which  the  sub  will  abort  its  mission  to  rearm. 

•  whether  the  sub  will  transmit  the  detections  it  makes  and  risk  detection  itself  or 
not  report  any  detections. 

•  the  interval  at  which  it  copies  the  submarine  broadcast  for  orders  to  investigate 
cues.  [Ref.  2:p.  2-37] 
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The  submarine’s  sensor  requires  a  detailed  set  of  parameters  of  its  own  which 
will  be  used  to  construct  the  contact  report  that  is  communicated  to  the  ACT 
representing  the  data  fusion  center  associated  with  the  submarine’s  command  object: 

•  position  error  which  represents  the  2  standard  deviation  radius  of  a  circular 
normal  distribution  of  contact  errors  about  the  actual  target  position. 

•  course  and  speed  errors  which  are  uniform  about  the  true  value. 

•  Target  Motion  Analysis  (TMA)  delay  representing  the  time  required  to  acquire 
course  and  speed  information. 

•  false  alarm  rate. 

•  determination  whether,  on  the  average  the  sensor  is  reliable  enough  for  the 
fusion  center  to  automatically  start  a  new  track  based  on  a  single  contact  report. 

The  complexity  of  the  data  involved  in  constructing  an  architecture  for 
evaluation  varies  considerably,  as  can  be  s^n  from  these  examples.  Once  all  this 
information  is  input,  it  can  be  easily  double  checked  by  accessing  the  appropriate 
windows  again.  When  the  user  is  sure  all  data  is  input  correctly,  the  simulation  can 
be  run  with  the  platform  graphics  turned  on  to  ensure  it  progresses  properly.  These 
graphics  can  then  be  turned  off  to  run  multiple  iterations  faster. 

When  the  desired  number  of  iterations  is  completed  a  dialog  box  opens  asking 
if  the  MOE’s  are  to  be  saved.  Once  saved  to  disk,  they  can  be  opened  and  examined. 
They  include; 
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•  Attrition  of  submarines  and  MPA  assets  for  each  side  as  a  fraction  of  original 
force. 

•  Number  of  times  submarines  approached  to  within  a  critical  range  (assigned  by 
the  user)  of  enemy  surface  formations  (which  serve  only  as  targets  in  ASSET 
1.0,  they  have  no  combat  or  detection  capability). 

•  Tracker  statistics  involving  the  fusion  delay  involved  in  constructing  target 
tracks. 

These  statistics  along  with  weapon  expenditures  allow  the  completed  simulation 
to  be  compared  to  other  runs  to  observe  the  effect  of  a  particular  parameter  changing, 
or  how  close  a  particular  MOE  comes  to  a  goal  value.  The  methodology  of  ASSET 
must  be  taken  into  account  as  the  computer  commanders  follow  simple  resource 
allocation  rules  which  do  not  necessarily  reflect  the  tactical  priorities  the  user  desires. 
A  higher  priority  for  prosecution  may  well  be  given  to  a  submarine  returning  to  base 
than  one  ten  miles  from  a  surface  group,  due  to  the  resource  allocation  process’ 
emphasis  on  detection  rather  than  on  protection.  While  creativity  is  needed  in  the 
design,  implementation  and  interpretation  of  an  ASSET  scenario,  significant  insights 
into  the  conduct  of  ASW  campaigns  can  be  gleaned  from  it. 

3.  Correlation,  Tracking  and  Data  F^ion. 

ASSET  is  designed  to  simulate  the  data  fusion  centers  where  contact  reports 
from  a  wide  variety  of  sensors  and  platforms  are  centrally  processed  to  create  a 
tactical  picture  for  the  region  of  interest.  These  data  fusion  centers  are  simulated  by 
an  instance  of  the  OBU-ACT  module  with  appropriate  communication  connectivities. 
The  result  of  correlating  and  processing  the  streams  of  contact  information  arriving  at 
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a  fusion  center  forms  the  basis  for  the  allocation  and  cuing  of  assets.  Thus  it  is 
critical  that  the  ACT  is  not  spinning  its  wheels  performing  unnecessary  computations. 

The  implementation  of  OBU-ACT  used  in  ASSET  is  organized  into  four  main 
functional  areas: 

•  generating  a  new  track  from  one  or  more  contact  reports  (the  START  and 
CLUSTER  modules). 

•  associating  a  contact  report  to  an  existing  track  (the  INPUT,  START,  and 
ASSOC  modules). 

•  simulating  the  intervention  of  a  human  analyst  to  resolve  ambiguous  contact-to- 
track  associations  (the  ANALYST  module). 

•  updating  and  managing  a  database  of  the  status  of  all  contact  reports  and  tracks 
(the  Locational  Data  Base  Manager  and  MTST  modules). 

The  data  flow  through  these  modules  is  shown  in  Figure  1.  A  detection  is  made 
by  a  sensor  and  a  report  is  initiated.  After  the  applicable  delays  have  elapsed,  the 
contact  report  travels  along  the  communications  network  to  the  fusion  center, 
experiencing  additional  delay  at  each  node. 

Upon  arrival  at  the  fusion  center  the  INPUT  module  enters  it  into  the  Locational 
Data  Base  Manager  (LDBM)  module  and  passes  it  to  the  ASSOC  module.  There  the 
contact  is  compared  to  the  existing  tracks  on  the  basis  of  geofeasibility.  Those  tracks 
with  which  the  contact  could  be  associated  are  evaluated  by  a  statistical  comparison  of 
a  measure  of  correlation  (MOC)  with  a  preset  threshold.  If  the  contact  fails  to  trigger 
an  association  with  any  track  it  is  passed  to  the  START  module.  If  the  contact 
triggers  a  single  association,  or  if  one  track  MOC  exceeds  all  others  by  a  preset 
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margin,  it  is  unambiguously  associated  with  that  track  [Ref.  4:p.  3].  A.mbiguous 
association  is  resolved  by  the  ANALYST  module,  which  has  a  given  probability  of 
making  the  correct  association  (.7)  and  an  associated  exponentially  distributed  delay 
lime  spent  making  the  decision  (mean  of  .2  hours)  [Ref.  3:p.  11;  Ref.5: ACT- 
ANALYST]. 
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The  contacts  which  are  unresolved  are  passed  to  START  which  checks  the  value 
of  the  flag  single-report-to-track.  If  the  value  is  true,  START  initiates  a  new  track 
based  on  the  contact,  if  it  is  false,  the  contact  is  passed  to  the  multiple  contact  track 
initiation  module  CLUSTER.  This  represents  a  simplification  of  the  actual  OBU- 
ACT  START  module  which  uses  comparison  to  a  set  of  criteria  and  human  analyst 
interaction  to  determine  if  a  new  track  is  to  initiated  [Ref.  6:p.  3-4].  Within  the 
context  of  the  simulation,  this  simplification  makes  sense  as  two  classes  of  detecting 
sensors  are  represented;  those  that  report  continuous  contact  observations  such  as 
SOSUS  or  other  fixed  area  sensors,  which  are  processed  at  the  fusion  center;  and 
those  which  perform  platform-level  analysis  and  periodically  report  the  tracks  they 
hold,  such  as  submarines.  Platforms  in  the  Erst  category,  unless  possessing 
unusually  low  false  alarm  rates,  would  not  initiate  a  track  based  on  a  single  report, 
while  those  in  the  second,  would. 

Unassociated  contacts  which  remain  after  passing  through  the  ASSOC  and 
START  modules  are  processed  by  the  CLUSTER  module.  Here  subsets  of  all 
unassociated  contacts  are  evaluated  for  geofeasibility  and  constant  cc^^rse  and  speed 
likelihood.  If  the  value  of  the  constant  course  and  speed  likelihood  exceeds  a 
threshold  value  then  the  subset  of  unassociated  contacts  is  used  to  initiate  a  new  track 
[Ref.  7:p.  1-3].  Contacts  which  are  not  associated  after  passing  through  ASSOC  and 
CLUSTER  carry  counters  which  increment  each  time  they  pass  through  the  two 
modules.  When  these  counts  exceed  maximum  limits  (hardwired  at  7  in  CLUSTER 
and  at  5  in  ASSOC)  they  are  dropped  from  the  LDBM  [Ref  5:  OBU-ACT]. 
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When  a  new  track  is  initiated  or  a  contact  is  associated  with  a  track,  the  MTST 
module  is  called  to  filter  the  track  data  and  produce  an  optimal  estimate  of  the  target’s 
location  and  velocity  along  with  the  error  covariance  matrix  describing  the  quality  of 
the  estimate.  The  full  capability  tracker  in  MTST  contains  the  matrix  versions  of  the 
Kalman  filter  equations.  These  equations  determine  the  mean  target  position  at  a 
future  point  in  time  and  the  covariance  matrix  representing  the  error  in  that 
prediction.  The  elements  of  this  covariance  matrix  can  be  recast  as  ellipses 
representing  two  a  or  86  percent  containment  regions  for  the  target’s  possible  position 
and  velocity.  A  complete  description  of  MTST  is  the  subject  of  Chapter  n. 

The  foregoing  process  of  correlating  contacts  to  tracks  and  filtering  those  tracks 
to  provide  an  estimate  of  target  position  occurs  at  each  instance  of  a  data  fusion 
center  object.  When  formulating  an  architecture,  the  fusion  center’s  role  must  be 
carefully  designed.  Since  no  contact  reports  flow  out  of  a  fusion  center,  it  is  not 
possible  to  directly  fuse  the  pictures  at  two  or  more  fusion  sites  into  a  higher  echelon 
center.  The  individual  sensors  originating  the  detection  reports  must  send  duplicate 
reports  to  all  fusion  centers  which  will  be  using  the  report  in  determining  a  tactical 
picture.  The  fusion  center  also  keeps  separate  instances  of  the  tracker  to  process 
surface  contacts  and  subsurface  contacts.  Together  with  the  START  processing  of 
single  contact  to  track  decisions,  this  assumes  that  a  great  deal  of  contact  processing 
is  going  on  below  the  level  of  the  fusion  center. 

Assuming  that  reports  arriving  at  the  fusion  center  are  pre-processed  justifies 
several  assumptions  that  the  DETECTION-REPORTER  object  makes  about  the 
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reports  it  generates.  Most  important  to  streamlining  filter  operation  is  the  assumption 
that  all  submarine  probability  areas  (SPA’s)  are  circular.  While  this  represents  a 
considerable  inaccuracy  for  single  contact  line-of-bearing  detections,  if  one  assumes 
the  contact  report  represents  the  accumulation  of  a  significant  number  detections,  the 
circular  error  assumption  is  more  reasonable.  Several  types  of  detections  are  resolved 
in  ASSET  without  explicitly  modelling  the  geometry  of  the  searcher  and  target  at  the 
time  of  detection  (MPA  and  Fixed  Area  Sensors  specifically).  The  calculation  of  the 
size  and  orientation  of  explicit  AOU  ellipses  where  possible  would  slow  execution  and 
add  considerable  complexity  to  the  program,  requiring  even  more  specific  user  input 
sensor  parameters. 

The  assumption  that  this  circular  AOU  size  is  independent  of  range  is  more 
difficult  to  defend,  in  light  of  an  expected  linear  relationship  between  the  standard 
deviation  of  the  position  error  and  range.  That  assumption  on  ASSET’S  part  can  be 
remedied  easily,  for  the  cases  where  range  is  explicitly  determined,  by  expanding  the 
radius  of  a  sensor’s  AOU  linearly  with  range  based  on  a  user  input  bearing  error. 

This  would  make  the  process  of  tabulating  preprocessed  variance  data  impossible, 
however,  as  the  sensor  AOUs  would  no  longer  be  constant.  A  good  compromise 
involves  defining  three  AOU  sizes  for  contacts  at  short,  medium  and  convergence 
zone  ranges.  This  would  allow  for  some  representation  of  the  range  effect  on  AOU 
size,  with  acceptable  complication  of  the  variance  pre-calculation  procedure. 

The  other  tracking  assumption  ASSET  makes  is  that  the  covariance  of  the 
velocity  can  be  represented  by  the  steady-state  stationary  cross-covariance  of  the  lOU 
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process,  independent  of  the  sensor  characteristics.  This  assumption  is  used  to 
initialize  the  covariance  matrix  of  a  new  track.  This  method  is  correct  for  a  position- 
only  contact  which  gives  no  information  about  velocity.  Given  position  and  velocity 
information  however,  this  is  only  accurate  if  the  sensor  is  so  inaccurate  that  the  lOU 
process  velocity  variance  is  small  compared  to  the  sensor  velocity  variance  [Ref.  8:p. 
5-20]. 

This  is  not  true  in  most  cases  however,  with  the  result  being  that  the  filter  re¬ 
initializes  the  velocity  update  each  time  a  contact  is  processed,  never  including  any 
representation  of  the  velocity  accuracy  of  the  sensor  involved.  Thus,  the  velocity 
variance  is  based  solely  on  the  lOU  model  and  not  on  the  variance  of  the  velocity 
reported  by  the  sensors.  This  assumption  makes  the  tracker  more  responsive  to 
course  changes,  but  it  also  artificially  inflates  the  SPA  size  of  accurate  contact 
streams  because  it  ignores  the  sensors  velocity  error,  which  may  be  significantly  less 
than  the  lOU  value. 

Given  these  assumptions  and  the  noise  characteristics  of  the  lOU  process,  the 
filter  implementation  can  be  streamlined  to  better  match  the  data  provided.  This 
process  is  detailed  in  the  following  chapters  which  will  describe  the  lOU  process  and 
the  formulation  of  the  general  and  ASSET-speciflc  Alter  equations. 
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n.  THE  MANEUVERING  TARGET  STATISTICAL  TRACKER 


Maneuvering  Target  Statistical  Tracker  (MTST)  is  the  name  given  to  a  class  of 
Kalman  filter-based  estimation  and  prediction  algorithms  utilizing  the  Integrated 
Omstein-Uhlenbeck  process  to  model  the  statistics  of  target  motion.  To  successfully 
track  a  maneuvering  target,  the  details  of  the  target  path  should  be  statistically 
predictable.  How  accurately  an  automated  tracker  like  MTST  performs  its  task  is 
directly  related  to  how  well  the  tracker’s  target  motion  model  describes  the  targets 
actual  random  motion. 

The  random  tour  model  [Ref.  9]  can  be  used  to  describe  the  statistical  properties 
of  a  target  moving  at  constant  speed  which  makes  random  course  changes  at  times 
separated  by  intervals  chosen  from  an  exponential  distribution.  This  provides  a 
reasonable  estimate  of  the  type  of  motion  expected  of  a  patrolling  submarine  target. 
Unfortunately,  the  target  distribution  generated  by  a  random  tour  is  not  normal  and 
cannot  be  used  directly  with  a  Kalman  filter  [Ref.  8:p.  2-10].  A  process  is  required 
which  produces  normal  distributions  which  best  approximate  the  mean  and  variance  of 
the  random  tour.  This  is  the  lOU  process.  The  mathematical  development  which 
follows  is  summarized  from  the  more  complete  treatment  in  Reference  8,  the  results 
of  which  are  in  agreement  with  References  5,  10  and  13. 
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A.  THE  INTEGRATED  ORNSTEIN-UHLENBECK  PROCESS 


The  Integrated  Omstein-Uhlenbeck  Process  (lOU)  is  based  on  a  first  order 
stochastic  linear  differential  equation  which  approximates  the  higher  order  non-linear 
deferential  equation  which  defines  the  motion  of  a  randomly  maneuvering  target.  It  is 
a  member  of  a  class  of  equations  known  as  Langevin  equations  and  has  the  form: 

--u(t)  =  u(t)dt  +  a  dw{t).  (2.1) 

at 

The  first  term  represents  a  deceleration  caused  by  a  resistive  force  proportional  to  the 
velocity  u(t).  The  random  forces  acting  on  the  particle  are  represented  by  the  second 
term,  where  w(t)  is  a  Gaussian  white  noise  process. 

The  stochastic  nature  of  this  equation  makes  it  possible  to  find  only  the  statistics 
of  the  distribution  of  the  solutions,  rather  than  the  specific  solution  itself.  A  Gaussian 
w(t)  produces  a  Gaussian  velocity  process  u(t)  which,  like  any  normal  process,  is 
defined  completely  by  its  first  and  second  order  moments.  The  result  is  the  velocity 
distribution  of  a  particle  which  is  undergoing  random  motion  similar  to  Brownian 
motion,  experiencing  random  instantaneous  accelerations,  but  whose  velocity  is 
damped  by  a  spring-like  effect  which  constantly  accelerates  the  particle  in  tlie 
direction  opposite  its  velocity  at  a  rate  proportional  to  that  velocity.  The  position 
variance  of  such  a  particle  is  unbounded  over  time,  but  the  velocity  variance  is 
limited  by  the  damping  coefficient  0.  The  result  is  a  velocity  distribution  HaL  i& 
normal  with  a  limiting  variance  given  by: 
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(2.2) 


limV{uit)}  =  lim£{[w(0-£{u(0}rt  =  4’ 

h*co  h*»  Zp 

and  a  mean  for  any  given  t  given  by: 

£{«(/)}=  ^-^£{1/(0)}.  ^2.5; 

The  mean  and  variance  expressions  for  the  position  of  an  object  whose  motion 
is  governed  by  an  lOU  process  completely  specify  its  positional  distribution  over 
time.  In  order  to  show  that  the  lOU  process  approximates  the  same  motion  as  a 
random  tour,  the  radial  position  distributions  for  the  two  stochastic  processes  will  be 
shown  to  have  the  same  variance. 

B.  EQUIVALENCE  OF  THE  RANDOM  TOUR  AND  lOU  PROCESSES 
The  variance  of  the  radial  distance  from  ()c(0)yy(0))  is: 

£{r2(0}  *  £{Wr)-Jc(0)r-^[yW->’(0)]'}. 

For  the  Random  Tour,  the  following  holds: 

£{£2(,)}  =  (2.5) 

where  V  is  the  speed  of  the  randomly  touring  particle  and  a  is  the  mean  number  of 
course  changes  per  unit  time.  The  corresponding  result  for  the  lOU  process  is: 
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a.  6) 


E{R\t)}  =  ^{fit-Ue-% 

where  a  is  the  scale  factor  for  the  random  acceleration  and  /3  is  the  damping  constant 
on  the  velocity,  as  in  Equation  (2.1). 

These  two  equations  can  be  made  identical  given  the  following  relationships 

hold: 

=  a  (2.7) 

Thus,  a  Random  Tour  with  parameters  a  and  V  can  be  approximated  by  an  lOU 
process  with  parameters  /S  =  a  and  a  =  It  is  worth  noting  that  since  a  normal 

distribution  is  completely  specified  by  its  mean  and  variance,  the  lOU  process 
represents  the  best  normal  approximation  to  the  Random  Tour  [Ref.  10]. 

In  ASSET  the  lOU  parameters,  and  a  are  hardwired  to  represent  a  target 
conducting  a  random  tour  at  ten  knots  with  an  average  time  between  course  changes 
of  four  hours.  These  parameters  should  match  the  actual  motion  of  the  platforms 
modelled  in  a  given  scenario.  Since  the  user  can  choose  whatever  target  motion 
parameters  he  wants  for  each  individual  platform,  the  parameters  embedded  in 
ASSET  may  disagree  considerably  with  the  actual  target  motion.  This  disagreement 
causes  the  lOU  prediction  to  consistently  lead  or  lag  the  target’s  mean  position, 
depending  on  whether  the  model’s  speed  and  course  change  rate  correspond  to  a 
velocity  faster  or  slower  than  the  target’s  speed,  respectively.  This  causes  the 
position  of  the  center  of  the  resulting  SPA  to  be  offset  from  the  true  target  position. 
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Even  when  the  tracker  parameters  exactly  match  the  target  motion,  a  considerable  lag 
occurs  if  the  time  between  measurements  is  not  significantly  greater  than  the  time 
between  maneuvers. 

C.  OTHER  MOTION  MODEL  OPTIONS. 

The  lOU  model  used  by  MTST  damps  out  the  mean  speed  of  the  target 
exponentially  over  time  to  account  for  target  maneuvers.  No  matter  how  small  the 
time  interval,  the  process  reduces  the  mean  speed  by  the  proper  amount.  If  the 
interval  between  detections  is  consistently  shorter  than  the  maneuver  interval,  the 
damped  velocity  is  used  to  predict  the  position  of  the  constant  velocity  target  and  the 
tracker  develops  a  lag.  One  remedy  for  these  inconsistencies  would  involve 
increasing  the  complexity  of  the  tracker  by  using  adaptive  methods  to  more  closely 
match  the  model  parameters  to  a  given  target’s  track.  An  adaptive  filter  recognizes 
changes  in  the  targets  motion  and  compensates  for  it  by  changing  the  parameters  of 
the  motion  model. 

Desiring  to  reduce  the  filter’s  complexity  however,  instead  of  explicitly 
modelling  an  adaptive  tracker,  the  use  of  such  a  tracker  can  be  approximated  using 
the  lOU  process  as  a  basis  for  the  size  of  the  SPA,  but  assuming  the  model  has  some 
adaptive  properties  to  position  it  more  accurately.  This  can  be  accomplished  using 
the  existing  lOU  model  to  estimate  the  SPA  variance  and  by  assuming  the  tracker’s 
position  prediction’s  are  distributed  about  the  target’s  true  position  in  a  circular 
normal  fashion.  This  is  an  optimistic  assumption,  but  captures  the  fact  that  real  world 
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trackers,  using  more  complete  contact  data  than  ASSET  and  an  adaptive  motion 
model,  can  achieve  more  accurate  position  and  velocity  estimates.  Examinations  of 
such  trackers  can  be  found  in  References  14,  IS  and  16.  Reference  14  in  particular 
demonstrates  a  passive  tracker  that  delivers  outstanding  accuracy  while  the  contact  is 
on  a  constant  course  and  speed,  but  has  difficulty  maintaining  track  during  difficult 
target  maneuvers. 

The  benefits  of  adaptive  tracking  are  not  applied  to  the  SPA  size  in  this  case,  as 
the  ASSET  tracker  is  not  an  adaptive  one,  but  are  assumed  in  simplifying  how  its 
position  is  determined.  This  technique  introduces  a  moderate  degree  of  error  in  SPA 
size  by  not  precisely  simulating  the  mechanics  of  distributing  the  position  of  the 
center  of  the  SPA.  It  also  does  not  take  into  account  the  filter’s  velocity  components. 
Reference  15  provides  methods  for  countering  these  effects  through  noise  adaptation 
and  correlated  maneuver  gating  while  Reference  16  uses  bias-sensitive  maneuver 
detection  and  Kalman  gain  adaptation.  Another  option  used  to  improve  the  lOU 
model  is  the  use  of  a  dual  velocity  system  which  combines  a  short  term  lOU  process 
combined  with  a  long  term  one.  This  type  of  model  can  reproduce  a  variety  of 
motions  depending  on  the  weighting  factors  given  to  the  short  and  long  term 
components.  The  resultant  Kalman  filter  has  six  states  and  a  6  x  6  covariance  matrix. 
This  is  the  implementation  of  the  lOU  process  currently  used  in  the  TOMAHAWK 
fire  control  system  for  surface  ship  motion  modelling  [Ref.  8].  These  are  but  a  few 
of  the  schemes  more  complex  trackers  can  use  to  improve  motion  modelling.  The 
single  velocity  lOU  process,  while  not  adaptive,  comes  close  to  satisfying  the 
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necessary  conditions  and  since  it  is  the  basis  of  the  ASSET  tracker  will  be  used  for 
comparison. 

D.  THE  KALMAN  FH^TER 

In  order  to  estimate  target  position,  a  method  of  combining  the  contact 
position  and  its  covariance  with  a  prediction  of  that  position  and  covariance  based  on 
the  lOU  process  is  necessary.  The  Kalman  Filter  provides  such  an  estimate,  provided 
the  process  model  is  linear  and  time  invariant,  which  the  lOU  process  is.  The 
Kalman  filter  is  derived  from  the  least-squares  estimation  theory  of  Gauss  and 
Legendre  [Ref.  ll:p.  7].  This  technique  sought  the  most  probable  value  of  an 
unknown  quantity  based  on  a  series  of  measurements  containing  unknown  errors. 
More  formally,  finding  the  most  probable  estimate  of  x  based  on  the  matrix  equation: 

where  vector  represents  a  measurement  taken  at  a  discrete  time  k,  based  on  an 
observation  matrix  H  revealing  information  about  one  or  more  state  parameters  of  the 
true  state  vector  x  but  corrupted  by  a  noise  vector  v.  This  technique  was  generalized 
by  Kalman  to  apply  to  linear  filter  theory,  producing  a  filter  which  produced  a  best 
linear  estimate  of  x  [Ref.  12]. 

The  resulting  filter  is  based  on  two  mechanisms  operating  sequentially,  one 
predicting  the  state  and  covariance  for  a  future  time  and  the  other  combining  this 
prediction  with  a  noisy  measurement  of  the  target’s  state  taken  at  that  time.  This 
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should,  given  correct  motion  modelling  and  noise  parameters,  produce  a  state  and 
covariance  which  more  accurately  reflects  the  true  target  state  than  either  the 
prediction  or  the  measurement  alone. 

These  mechanisms  are  represented  by  two  systems  of  matrix  equations.  The 
variables  involved  are  defined  in  Table  1.  The  prediction  equations  for  the  state  x 
and  covariance  T  are: 

state  prediction:  (2.9) 

covariance  prediction:  + (2.10) 

where  <t>  represents  the  transition  matrix  from  the  dynamic  system  model,  Q 
represents  the  variance  inherent  in  the  dynamic  model  and  /x«  represents  the  mean  of 
the  noise  term  w  from  the  dynamic  model,  which  is  assumed  to  be  zero  in  ASSET. 
The  equations  for  combining  the  predicted  and  measured  states  and  variances  require 
the  computation  of  a  Kalman  gain  matrix  which  provides  the  weighting  factors  for  the 
combination.  These  equations  which  update  the  filter’s  estimates  are: 

Kalman  Gain: 

state  update:  x*,,  = 
covariance  update: 

where  R  represents  the  variance  of  the  noise  inherent  in  the  measurements,  Z  is  the 
measurement  itself,  ^  is  the  mean  of  the  noise  inherent  in  the  measurement  (also 


a.ii) 

(2.12) 

(2.13) 
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Table  I:  Kalman  Filter  Definition  of  Terms 


Kalman  Term: 

General  Form 

ASSET  Form 

State  vector  at  time  k: 

X*  n  x  1 

mean  4x1 

State  transition  matrix, 
time  k  to  time  k+1: 

^  n  X  n 

phi  4x4 

State  process  noise  mean: 

n  X  1 

assumed  zero 

State  error  covariance, 
at  time  k: 

Ek  n  X  n 

var  4  X  4 

State  process  error 
covariance  at  time  k: 

Ck  n  X  n 

f  4  X  4 

Observation  matrix: 

m  X  n 

14x4 

Observation  at  time  k: 

m  X  1 

state  4x1 

Observation  error  mean: 

/Xv  m  X  1 

assumed  zero 

Observation  error  covariance 
at  time: 

/?k  m  X  n 

covariance  4x4 

assumed  to  be  zero  in  ASSET)  and  I  is  the  identity  matrix  of  proper  size.  The  state 
and  covariance  updates,  assuming  H  (as  ASSET  does)  is  the  appropriate  identity 
matrix  can  be  written  as: 


state  update:  x*,,  = 


a.  14) 


covariance  update:  E*.,  =  ,  (2.15) 

where  the  inverse  of  and  E^^,  are  the  weighting  matrices.  Thus  the  inverse  of 
the  noise  error  variances  are  the  weighting  factors  by  which  the  prediction  and 
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measurement  are  linearly  combined.  This  is  the  form  of  the  Kalman  equations  which 
ASSET  actually  uses.  The  advantage  of  this  form  is  the  ready  understanding  of  the 
underlying  mechanism  of  weighted  linear  combination.  The  disadvantage  is  the 
computational  necessity  of  performing  three  matrix  inversions  to  calculate  the 
variance  propagation.  Regardless  of  the  form  of  the  equations,  the  <t>,  Q  and  R 
matrices  must  be  developed  to  complete  the  computational  form  of  the  filter. 

1.  Development  of  the  ^  Matrix. 

The  matrix  4>  represents  the  transition  matrix  which  governs  the  dynamics  of  the 
state  between  two  discrete  times.  The  following  development  summarizes  the 
derivation  of  Reference  11,  Section  5.2.  The  differential  equations  defining  the  lOU 
process  and  velocity  can  be  combined  to  form  a  single  matrix  differential  equation 
which  takes  the  following  form  in  the  single  coordinate  direction  x: 


MO 

0 

1 

xit) 

0 

0 

u{t) 

a 

w(t) 


(2.16) 


more  compactly:  X{t)  -  F{t)  X{t)  G  wit) 

where  Xrtjrepresents  the  system  state  vector  and  the  dot  over  a  symbol  indicates 
differentiation  with  respect  to  time.  Given  that  X(t)  has  the  value  X(tJ  at  time  4  then 
X(t)  at  any  future  time  /  >/„  is  given  by: 
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(2.18) 


m  =  ^  I  <t>(r,r)Gw(r)dT 

where  4>(t,to)  represents  the  transition  matrix  between  the  states  at  time  r<,  and  time  t. 
Provided  that  F(t)  is  constant  in  time  and  assuming  that  4  is  zero,  the  matrix  <t>  can  be 
shown  [REF.  8:p.  5-3, 5-5]  to  have  the  form: 

I 

m  = 

0  €-^' 

This  represents  the  transformation  projecting  a  state  into  the  future  based  on  the  lOU 
model.  The  velocity  mean  decays  to  a  new  value  which  is  simply  the  old  value 
multiplied  by  e'^‘.  The  position  is  moved  a  distance  equal  to  the  time  l/jS  multiplied 
by  the  change  in  velocity; 

*  "(Of,)’ 
p 

where: 

=  u(t)^e-^.  (2.21) 

The  form  this  takes  when  expanded  to  cover  both  coordinate  directions  is; 


1  0  0 
=0  1  0  ^(1-^-^)  .  ^2.22; 

0  0  €-^‘  0 

0  0  0  e-^‘ 

where  t  is  time  elapsed  from  the  last  measurement  to  the  observation  time  of  the 
current  measurement.  It  is  important  to  notice  that  the  transition  matrix  depends  only 
on  the  lOU  parameter  j3  and  not  on  a.  Thus  the  lOU  velocity  damping  coefficient 
and  the  elapsed  time  determine  the  dynamics  of  the  mean  state  and  variance 
transition.  Appendix  A  contains  graphs  of  the  values  ^(1  2),  which  will  be  called  ^2, 
and  ^(2), which  will  be  called  <f>3,  take  over  various  time  intervals.  The  noise 
magnitude  coefficient  a  is  involved  in  the  noise  terms  R  and  Q. 

1,  Development  of  the  Q  Matrix. 

The  Q  matrix  represents  the  noise  introduced  by  the  white  Gaussian  process 
w(t).  As  discussed  above,  as  the  elements  of  this  matrix  get  large  the  filter 
increasingly  relies  on  the  measurement  data  and  ignores  its  predictions.  As  the 
elements  get  small  the  filter  increasingly  ignores  the  new  contact  data  and  the  errors 
in  its  predictions  compound  until  it  diverges,  completely  losing  track  of  where  its 
target  is.  The  definition  of  this  matrix,  the  process  noise  error  covariance,  is: 

Q  =  £[{Z-E(Z)}{Z-£(Z)}i, 

which  for  the  random  process  operating  in  (2.26)  becomes: 
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Since  the  mean  of  the  white  Gaussian  processes  w(t)  and  w(sj  are  both  zero  and  the 
variables  of  integration  t  and  s  are  equal,  this  reduces  to  the  form: 


Substituting  the  appropriate  matrices  G  and  <i>  from  (2.25)  and  (2,29)  respectively  and 
performing  the  indicated  matrix  multiplication  results  in: 


f^(l_g-<»(«-))2 

^2' 


J^(l_g-«»-))^-«/-r) 


for  the  single  coordinate  direction  x.  Assuming  that  4=0  and  recognizing  that  q(l,2) 
is  equal  to  q(2,l)  results  in  three  expressions  to  integrate  to  get  the  final  value  of  the 
Q  matrix  in  one  direction: 


qll  ql2 
q21  q22 


where  ql2  =  q21 


qll  =  -fLr2/-I(3-4e-<*'  +  e-^') 

20^  [  &  J 


(2.28) 
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a. 29) 


qI2  ^q2I  =  ^(l  -  2e *  e -») 

q22‘^{l-e-»)  (2-30) 

2p 

When  projected  into  two  coordinate  directions  the  full  representation  of  the  Q  matrix 
is: 

‘qll  0  ql2  O' 

0  qll  0  ql2 

Q  =  (2.31) 

ql2  0  q22  0 

0  ql2  0  q22_ 

Each  element  in  this  matrix  is  directly  proportional  to  and  decreases  with 
increasing  /3.  Time  has  an  inverse  exponential  relationship  causing  the  noise  to 
increase  the  longer  the  interval  between  measurements.  At  t  equal  to  zero  the  noise 
terms  are  equal  to  zero  and  as  t  goes  to  infinity  : 

q/f-oo,  ql2--^,  q22^^.  (2.32) 

As  mentioned  previously,  the  variance  in  position  is  unbounded,  but  the  cross- 
covariance  and  velocity  covariance  are  bounded.  These  values  become  important  in 
developing  the  measurement  noise  covariance  matrix  R,  Appendix  A  contains  graphs 
of  these  values  over  various  time  intervals. 
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3.  Development  of  the  R  Matrix  and  Filter  Initialization. 

The  matrix  R  represents  the  covariance  of  the  measurement  noise  and 
accompanies  each  state  measurement  reflecting  its  precision.  This  value  may  vary 
with  time  in  the  ASSET  tracker  as  contact  reports  flow  in  from  a  variety  of  sensors. 
The  inverse  of  this  matrix  is  used  as  the  weighting  function  for  the  contact  report,  thf* 
larger  the  elements  of  R  the  less  effect  the  contact  has  on  the  filtered  update.  The  R 
matrix  also  plays  a  role  in  initializing  the  filter. 

When  a  target  is  first  detected,  there  is  no  /-7  state  for  the  filter  to  work  from  in 
making  its  prediction.  In  general,  a  time  R  matrix  is  preset  to  initiate  the  filter. 

This  matrix  would  consist  of  the  2  x  2  positional  error  matrix  which  describes  an 
estimate  of  the  elliptical  AOU  about  the  contact  position: 


I 

4 


(A ^ cos^B)  +  sin^0)  -A^)  sin0 cosB 

(B^  -A^) sinfl cos6  (A ^ sin^^)  +  (B^ cos^O) 


(2.33) 


For  a  position-only  contact,  the  two  lower  right  diagonal  entries  would  be  set  equal  to 
the  stationary  variance  of  the  lOU  process  (2.21.  This  represents  the  maximum 
variance  the  velocity  could  possibly  have  based  on  the  motion  model  and  since  no 
velocity  information  is  coming  in,  the  filter  is  relying  solely  on  the  motion  model, 
making  this  a  value  a  reasonable  choice: 
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EQ.  2.33 


0 

0 


0 

0 


0  0 
0  0 


0 


2a 


(2.34) 


If  the  CO  itact  is  a  position-and-velocity  contact,  the  procedure  is  more  complex.  In 
like  fashion  to  the  initialization  of  the  upper  left  2x2  portion  with  the  variance  in  the 
sensor  (its  elliptical  AOU),  so  must  the  bottom  right  2  x  2  be  initialized  by  the 
velocity  variance  of  the  sensor.  Since  the  lOU  process  is  predicting  the  velocity 
distribution  also,  this  variance  of  the  measurement  must  be  combined  with  the 
variance  of  the  lOU  process.  The  proper  combination  can  be  shown  [Ref.  8:App.  F] 
to  be  the  same  as  the  alternate  form  of  the  Kalman  covariance  update  equation  (2.14). 

This  method  is  not  used  by  the  tracker  in  ASSET.  The  ASSET  tracker  simply 
uses  the  initial  contact’s  lOU  velocity  variance  as  the  basis  for  a  new  track,  without 
updating  it.  This  original  contact  report  is  used  as  the  t-1  state  and  covariance  when 
the  next  contact  associated  with  the  track  comes  in.  The  contact  reports  themselves 
also  differ  from  the  construction  described  above.  The  positional  variance  of  the 
sensors  are  all  circles  in  ASSET  so  the  2  x  2  matrix  which  occupies  the  upper  left  of 
the  R  matrix  is  actually  the  2  x  2  in  the  upper  left  of: 
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4 

0  0^0 

2a 

0  0  0  -£l 

2a 


(2.35) 


The  velocity  variance  in  the  lower  right  hand  2  x  2  is  correctly  set  for  a  position-only 
contact,  but  this  is  also  used  for  a  position-and-velocity  contact.  Thus  the  R  matrix 
which  ASSET  uses  never  incorporates  the  error  in  velocity  inherent  in  the  sensor. 
Using  these  values  essentially  reinitializes  the  filter  each  time  a  measurement  is  made. 
This  is  desirable  for  position-only  contacts  as  it  prevents  divergence  of  the  velocity 
components  of  the  state,  however  for  position  and  velocity  contacts,  this  can  cause 
poor  velocity  accuracy  as  the  tracker  improperly  weights  the  velocity  components  of 
the  contact  report.  The  velocity  information  of  different  sensors  are  given  equal 
weight  regardless  of  the  relative  size  of  the  errors  in  the  reports  they  make.  The 
actual  velocity  variance  of  the  sensor  should  be  computed  and  used  to  set  the  value  of 
the  filtered  velocity  using  the  form  of  (2.15). 
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m.  IMPLEMENTATION  OF  THE  TRACKER  IN  ASSET 


Having  developed  the  components  of  the  Kalman  filter,  the  construction  of  the 
tracker  implemented  in  ASSET  can  be  discussed.  The  following  is  based  on 
Reference  5,  the  ASSET  source  code  for  version  1.0.  The  ACT-MTST  module 
consists  of  objects  which  perform  the  filter  calculations  described  above,  as  well  as 
objects  which  maintain  the  spatial  relationships  between  the  positions  involved  on  a 
spherical  earth. 

There  are  several  constants  which  are  defined  for  use  in  all  the  module’s 
objects.  The  first  is  dtor  which  is  used  for  degree  to  radian  conversion.  Where 
trigonometric  functions  are  indicated  below,  the  actual  code  uses  dtor  to  convert 
angles  from  degrees  to  radians,  but  in  the  interest  of  brevity,  this  will  be  excluded. 
Second  is  a,  which  is  equivalent  to  in  the  lOU  process,  and  is  set  to  .25.  Third  is 
a  which  is  the  noise  scale  factor  from  the  lOU  process  as  described  above  and  set  to 
VSQ.  This  is  equivalent  to  the  square  root  of  a  velocity  multiplied  by  the  Random 
Tour  velocity.  Thus  the  tracker  predicts  the  target  position  by  assuming  a  random 
tour  is  being  conducted  at  a  speed  of  ten  knots  with  a  mean  interval  between  course 
changes  of  four  hours.  This  cannot  be  changed  by  the  user. 

Inputs  to  ACT-MTST  are  read  from  the  data  in  ACT-LDBM,  the  contact 
database  manager.  If  a  new  track  is  to  be  initiated,  the  next  MTST  object,  start-new 
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track  sets  the  track  head  and  track  tail  state  and  covariance  matrices  equal  to  the 
contact  mean  and  covariance  using  compute-mean-covariance.  The  use  of  two  sets 
of  data,  a  head  and  a  tail,  to  allow  incorporation  of  out-of-sequence  contact  reports 
will  be  discussed  below.  The  object  compute-mean-covariance  is  also  called  by 
ACT-ASSOC  as  a  basis  for  the  spatial  measure  of  correlation  used  to  determine 
contact  to  track  correlation. 

Compute-mean-covariance  creates  two  arrays,  a  four  by  four  called  var,  which 
is  the  R  matrix,  and  a  four  by  one  called  mean,  which  is  4: 


mean 


0 

0 

spd  cos(90-cse)  ’ 
spd  sin(90-cje) 


(3.1) 


and  var,  which  has  the  same  form  as  (2.34).  The  vector  mean  is  centered  at  the 
latitude  and  longitude  indicated  in  the  contact  data  list  and  x  and  y  velocity  computed 
from  the  contact  course  and  speed.  This  variance  matrix  however,  as  discussed 
above,  is  calculated  as  if  the  contact  report  consists  of  an  ellipse  with  minor  axis  B, 
major  axis  A  and  orientation  angle  6.  All  the  spatial  data  fields  that  are  generated  by 
the  Detection  Reporter  and  Generic  Sensor  contain  a  single  positional  uncertainty  and 
an  orientation  angle  of  zero.  Modifying  (3. 1)  to  take  advantage  of  this  fact  results  in 
(2.35)  where  r  is  equal  to  the  standard  deviation  of  the  sensor’s  target  position 
estimates.  This  alternative  form  returns  matrix  element  values  identical  to  those 
computed  by  (3.1),  given  the  actual  form  of  the  contact  reports  generated. 
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If  a  contact  is  associated  with  an  existing  track  rather  than  used  to  start  a  new 
one,  the  update-track  object  is  called  by  ACT-LDBM  to  perform  the  Kalman  filter 
operations.  The  remaining  objects  in  ACT-MTST  are  all  components  of  the  update- 
track  object  with  the  exception  of  the  object  cov-to-ellipse,  which  is  called  by  various 
display  and  graphics  objects  to  determine  the  orientation,  semi-major  and  major  axes 
of  the  ellipse  representing  a  covariance  matrix.  Since,  as  shown  above,  all  the 
ellipses  are  actually  circles,  this  object  is  another  anachronism  of  the  mismatch 
between  the  capabilities  of  the  tracker  and  the  contact  reports  actually  input  to  it.  It 
can  be  simplified  in  a  manner  similar  to  compute-mean  covariance  above  to  return 
the  radius  of  a  circle  instead  of  a  major  axis,  minor  axis  and  orientation  of  a  ellipse. 

Focusing  once  again  on  the  update-track  object,  it  first  defines  the  spatial  data 
parameters  of  the  contact  and  the  track  the  contact  is  used  to  update.  ASSET  uses 
four  data  structures  to  represent  the  data  associated  with  contacts  and  tracks: 


•  obu-contact:  consisting  of  contact  id,  receipt  time,  track  association,  sensor, 
categorization  (track  association  flags  "pinn^p"  and  "lockedp",  and  number  of 
passes  through  ACT-CLUSTER  and  ACT-ASSOC),  spatial  data  (the  obu-data- 
field  described  below),  altitude  (surface  or  subsurface)  and  HFDFp  (an  optional 
parameter  associated  with  HFDF  contacts). 

•  obu-data-field:  type  (position-only  or  position-and- velocity),  observation  time, 
latitude,  longitude,  AOU  orientation,  AOU  major  axis,  AOU  minor  axis, 
course,  course  uncertainty,  speed  and  speed  uncertainty. 

•  obu-track:  track  id,  number  of  contacts  associated  to  the  track  (maximum  of 
five),  state  and  covariance  of  the  track  head  (most  recent  contact)  and  track  tail 
(oldest  online  contact),  head  and  tail  spatial  data  (each  an  obu-data-field),  head 
and  tail  contact  id,  and  altitude  (surface  or  subsurface). 
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•  obu-state-field:  reference  latitude,  reference  longitude,  state  array  (offset  from 
reference  position  in  miles  N/S,  offset  from  reference  position  in  miles  E/W, 
N/S  velocity,  E/W  velocity)  and  covariance  array  (four  by  four  array  per  (3.1)). 


The  three  AOU  variables  can  be  eliminated  from  obu>data-field  and  replaced  by 
position-uncertainty,  the  radius  of  the  actual  AOU  circle  reported.  This  will  not 
affect  the  tracker’s  calculation  and  reduces  the  memory  taken  up  by  contact  reports. 

An  obu-track  consists  of  up  to  five  contact  reports,  the  most  recent  of  which  is 
the  track  head  and  the  oldest  is  the  track  tail.  The  variables  required  for  performing 
the  filter  calculations  are  read  from  the  data  structures  described  above,  update-track 
then  checks  the  time  since  the  last  update.  If  the  new  contact  observation  time  is  later 
than  the  track  head  update  time,  the  contact  is  used  to  update  the  track  head  using  a 
set  sequence  of  object  calls.  This  sequence  contains  the  Kalman  filter  procedures  as 
well  as  two  routines  which  maintain  the  latitude  and  longitude  relationships  on  a 
spherical  earth  and  proceeds  as  follows: 


•  ioumotion:  Performs  the  lOU  prediction  step  and  updates  the  N/S  and  E/W 
offset  distances  in  the  track  head  state  to  the  time  of  observation  of  the  contact. 
These  offsets  effectively  perform  the  prediction  in  a  plane  tangent  to  the  earth’s 
surface  at  the  reference  latitude  and  longitude  point  of  the  track  head. 

•  change-tangent:  Finds  the  relationship  of  the  iou  predicted  position  and  the 
contact  point  in  a  plane  tangent  to  the  earth  at  the  contact’s  reference  point  vice 
the  track  head’s  reference  point  used  to  compute  the  prediction  in  ioumotion. 
These  matrices  are  stored  as  statecov-old.  Since  the  covariance  is  circular, 
there  is  no  need  for  the  sequence  which  rotates  it. 

•  make-state:  Takes  the  contact  data  and  produces  which  is  called  simply 
mean  and  its  covariance  R  which  is  called  var.  These  two  matrices  are  stored 
in  state-cov-ctc. 
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•  filter:  Takes  state-cov-old  and  state-cov-ctc  and  uses  Equations  2.14  and  2.  IS 
to  compute  and  The  result  is  the  Kalman  filter  update  performed  in  a 
plane  tangent  to  the  earth  at  the  contact’s  reference  point. 

•  center-tangent:  The  latitude  and  longitude  of  the  position  produced  in  filter  is 
projected  back  onto  the  earth  from  the  tangent  plane. 

•  update-track-head:  The  new  updated  state  and  covariance  are  made  the  new 
track  head.  The  contact  used  to  perform  the  update  provides  the  track-head 
contact  data. 

•  update-track-tail:  Sorts  the  contacts  making  up  the  track  in  time  order,  oldest 
to  most  recent  (1  to  n).  If  there  is  only  a  single  contact,  it  is  designated  the 
track  tail.  If  there  are  five  or  fewer  contacts  and  the  oldest  contact  is  already 
the  tail  it  remains  so;  otherwise  the  oldest  contact  is  made  the  new  tail.  If  there 
are  more  than  five  contacts,  it  takes  number  contacts,  subtracts  five  and  the 
contact  whose  number  equals  the  result  is  used  to  update  the  track  tail  using  the 
procedure  detailed  above.  The  contact  used  to  update  the  tail  provides  the  track 
tail  contact  data.  All  contacts  older  than  the  tail  are  then  pruned  from  the 
track. 


Considerable  computation  time  can  be  saved  by  simulating  a  portion  of  the  above 
process,  rather  than  actually  carrying  out  all  the  steps.  A  revised  update  procedure 
based  on  using  the  updated  variance  as  an  approximation  to  the  distribution  of 
updated  states  (Xk.».i)  is  proposed.  Rather  than  produce  the  contact  position  latitude 
and  longitude  randomly  using  the  sensor  variance  as  a  distribution  and  processing  it 
using  the  sequence  above,  the  latitude  and  longitude  are  chosen  by  computing  the 
updated  variance,  which  can  be  done  prior  to  scenario  execution.  When  it  is  needed, 
the  distribution  is  centered  on  the  target’s  true  position,  and  used  to  generate  the 
updated  states  directly  by  random  draw.  This  new  procedure  analyzed  in  detail  in 
Chapter  IV. 
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Time  delays  incurred  at  communications  nodes  enroute  to  the  fusion  center  may 
cause  the  observation  time  to  fall  between  the  track  head  update  time  and  the  track 
tail  update  time.  In  that  case  a  modified  procedure  is  followed  . 


•  If  the  time  of  observation  for  the  contact  is  later  than  the  tail  update  time  or  if 
the  track  does  not  yet  consist  of  five  contacts,  then  the  contact  is  appended  to 
the  track  and  the  contacts  are  sorted  by  time  of  observation.  If  the  contact  is 
older  than  the  tail  and  the  track  already  consists  of  five  contacts,  it  is  ignored 
and  the  procedure  stops. 

•  If  the  observation  time  is  later  than  or  equal  to  the  tail  update  time,  the  tail  is 
used  as  the  oldest  contact,  if  the  new  contact  is  older,  then  compute-mean-var 
is  used  to  make  the  new  contact  the  oldest. 

•  The  oldest  contact  is  then  updated  using  the  procedure  described  above  using  the 
next  contact  in  the  track,  in  time  sequence,  as  the  "contact".  The  result  of  this 
becomes  the  "old  contact"  and  the  proc^ure  is  repeated,  updating  each  contact 
in  the  track  in  sequence  until  the  head  is  updated.  The  resulting  "forward 
filtered"  track  closely  approximates  the  track  head  which  would  have  resulted  if 
the  Ume-late  contact  had  been  received  in  sequence. 

The  calculations  performed  in  ioumotion  and  filter,  all  assume  that  elliptical 

AOU's  are  being  produced  by  the  Detection  Reporter  and  General  Sensor  objects. 

The  object  make-state  can  be  simplified  in  the  manner  as  compute-mean-cov  as  it 

performs  the  same  operations.  The  reduced  forms  of  the  equations  in  those  objects 

are  computed  below. 


A.  REDUCED  COMPLEXITY  lOU  PREDICTION 


The  matrix  equations  for  calculating  the  lOU  predicted  state  and  covariance  are 
given  by  Equations  2.9  and  2.10.  Using  the  symbolic  processing  capability  of  the 
MathCad  3.0  (copyright  1991  MathSoft,  Cambridge  MA)  computer  software  package, 
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the  matrix  operations  were  evaluated.  The  large  number  of  zero  terms  and  high 
symmetry  of  the  and  Q  matrices  resulted  in  simple  expressions  for  the  elements  of 
the  state  and  variance  predictions.  Portions  of  the  calc-mot-mat  object  which 
compute  f(0,0),  f(0,2),  f(2,2)  for  positive  time  as  well  as  <^>(0,2)  and  0(2,2)  can  be 
used  to  produce  the  terms  of  the  following  equations  used  to  compute  new-var: 


where: 


where: 


i^^^=neMmean= 
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and  with: 
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A0,0) 
AO, 2) 
A2,2) 


^2  [4  ^ 


2/3^ 


The /tenns  given  here  are  the  ASSET  versions  of  (2.36),  (2.37)  and  (2.38). 

B.  REDUCED  COMPLEXITY  KALMAN  UPDATE 

In  a  manner  analogous  to  that  used  in  the  preceding  treatment  of  the  lOU 
update,  the  matrix  equations  for  the  Kalman  update  were  evaluated.  The  result  is; 


Kalman  Gain 


0 

0 

0 

0 

0 

Kc,., 

0 

0 

0 

where: 
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*3,2... 

Using  these  gain  values  the  corresponding  updated  variance  terms  are: 


Vb2,^,  *  Kb2,^,  ir^ 


[  J 

The  updated  state  is  written  in  terms  of  residuals  which  are  the  differences  between 
the  contact  states  and  predicted  states: 
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Residuals  = 


zy-^ 

zx-tc 

zVy-tVy 

zVx-iVx 


(3.10) 


S*KaRy*Kbl  RVy‘ 
i^KaRx^KblRVx 
y^^Kb2Ry*KcRVy\ 
Vx*Kb2Rx-^KcRVx\ 


(3.11) 


The  reduced  form  equations  for  the  state  are  included  here,  but  are  unnecessary  if  the 
estimation  of  the  state  is  to  be  used.  The  accuracy  of  the  estimation  technique  is 
analyzed  in  the  following  chapter. 
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IV.  MODinCATIONS  TO  THE  ASSET  TRACKER 


The  tracker  currently  used  in  ASSET  makes  two  major  assumptions  relating  to 
detection  modelling: 

•  Sensor  AOU’s  are  circles  vice  ellipses, 

•  The  velocity  variance  inherent  in  the  sensor  is  ignored  when  making  the  filtered 
variance  calculation  for  position  and  velocity  contacts. 


Figure  2  shows  the  geometry  of  a  typical  filter  operation: 


The  equations  which  exactly  determine  the  position  of  the  center  of  the  SPA  are  given 
in  (3. 12).  Given  that  the  simulation  knows  the  true  target  location  and  making 
several  assumptions  detailed  below,  it  is  proposed  to  bypass  the  Kalman  state  update 
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step  and  estimate  one  possible  position  of  the  SPA  based  on  the  relationship  between 
the  SPA  center  and  the  true  target  position.  If  the  true  position  is  distributed 
somewhere  in  the  SPA,  then  from  the  point  of  view  of  the  target,  the  SPA  center  is 
likewise  distributed  about  its  position.  This  allows  an  estimate  of  a  SPA  center  to  be 
generated  by  drawing  a  random  variable  from  a  distribution  with  standard  deviation 
equal  to  the  square  root  of  the  filtered  variance  in  position  with  a  mean  centered  on 
the  target’s  true  position.  Tliis  uses  the  SPA  "in  reverse"  to  find  a  possible  position 
for  its  own  center  based  on  the  targets  true  position.  Provided  the  necessary 
assumptions  are  met,  this  will  provide  a  reasonable  approximation  to  the  filtered  track 
over  a  series  of  observations.  It  also  allows  the  variance  values  to  be  precomputed 
and  drawn  from  a  table  vice  calculated  when  needed.  These  three  assumptions  are: 

•  The  mean  predicter"  position  and  velocity  must  be  very  close  to  the  target’s  true 
position  and  velo^  ty  so  the  AOU  of  the  predicted  position  is  concentric  with  the 
contact  AOU,  c.  nearly  so.  This  assumption  relates  to  the  motion  model  in  the 
tracker  simulated. 

•  It  is  sufficient  to  model  the  behavior  of  detections  in  the  mean,  rather  than 
explicitly  computing  them  individually.  This  assumption  relates  to  the 
interaction  between  the  correlator  and  the  tracker. 

•  The  distribution  of  filtered  states  has  its  mean  at  the  target’s  true  position  and  a 
variance  equal  to  the  steady-state  value  of  Va  (3.10). 

The  first  assumption  sets  a  requirement  for  the  accuracy  of  the  motion  model. 

If  the  predicted  position  is  consistently  offset  a  considerable  distance  from  the  target’s 
true  position  and  the  Kalman  gain  is  small,  then  the  center  of  the  filtered  position 
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distribution  will  be  drawn  off  the  target’s  true  position.  This  can  be  shown  by 
rearranging  the  terms  in  Equation  (3.12)  to  yield  for  a  single  dimension: 

y  =  y  (1  -Ka)  ^KaZy*  Kbl  (ZVy  ~  \y)  (^-  D 

The  filtered  position  y  approaches  the  contact  position  Zy  as  Ka  goes  to  one  and 
moves  off  to  the  predicted  position  ^  Ka  goes  to  zero.  This  case  must  be  avoided 
to  prevent  significant  inconsistencies  between  the  distribution  of  actual  filtered 
positions  and  the  estimates  generated.  Working  to  keep  this  relationship  is  the  effect 
shown  in  (2.5)  which  acts  to  push  the  position  toward  the  observed  position  as  the 
time  interval  increases  for  a  given  observation  AOU  size  and  the  lOU  prediction 
variance  gets  large  compared  to  it. 

This  effect  is  not  easily  identifiable  in  the  Kalman  gain  equations,  but  increasing 
the  time  interval  increases  the  size  of  the  predicted  AOU  and  increases  Ka  for  a  given 
observation  AOU  size.  This  counteracts  the  tendency  of  the  predicted  position  to 
recede  to  the  previous  filtered  position  as  ihe  time  interval  increases  and  keeps  the 
required  relationship  intact  for  cases  where  the  time  interval  between  observations  is 
large.  The  maximum  error  will  occur  when  the  two  AOU’s  are  nearly  the  same  size 
and  the  value  of  Ka  is  approximately  .5-.65.  This  assumption  is  also  involved  in  the 
computation  of  the  filtered  velocity,  where  it  is  assumed  that  the  difference  between 
the  predicted  and  observed  position  multiplied  by  gain  Kb2  is  small. 

The  second  assumption  requires  acceptance  of  a  lack  of  consistency  between  the 
correlator  and  the  tracker  models.  The  ACT-ASSOC  module  uses  the  Kalman  filter 
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for  calculation  of  the  spatial  measure  of  correlation  it  uses  to  associate  contacts  to 
tracks.  The  effect  of  using  equally  representative,  but  different,  implied  contact 
locations  by  the  correlator  and  tracker  to  do  contact  association  and  track  updating  has 
not  been  thoroughly  examined.  The  fact  that  the  filtered  position  produced  by  this 
method  does  not  relate  to  the  contact  position  used  for  track  association  does  not 
appear  to  be  a  serious  problem,  given  the  Monte  Carlo  nature  of  the  simulation.  The 
draw  used  by  the  correlator  can  be  looked  at  as  determining  whether  the  proper  track 
assignment  is  made,  a  false  assignment  made  or  no  assignment  made.  If  the  contact 
is  drawn  from  the  proper  distribution,  the  probability  that  any  single  draw  results  in 
any  of  these  occurrences  is  the  same  whether  or  not  the  tracker  goes  on  to  use  the 
contact  to  update  the  track  chosen. 

The  third  assumption  is  again  linked  to  the  accuracy  of  the  motion  model.  The 
result  of  making  this  assumption  is  shown  in  Figure  3.  The  first  part  is  essentially 
the  effect  of  assun  ption  one  being  true.  The  second  part  is  required  because  the 
variance  in  distribution  of  the  actual  filtered  position  is  affected  by  the  speed  of  the 
actual  target.  For  target  speeds  less  than  the  modelled  speed,  the  actual  distribution  is 
tighter  than  the  estimated  value.  The  opposite  is  true  if  the  target  speed  is  faster  than 
the  model.  The  implication  of  this  assumption  is  that  the  target  is  exactly  performing 
the  motion  modelled  and  the  tracker  does  not  respond  as  the  actual  Kalman  filter  does 
to  deviations  from  the  expected  moUon.  Since  the  range  of  velocities  the  submarine 
targets  may  have  is  small,  this  deviation  is  typically  less  than  fifteen  percent  of  the 
SPA  size. 


47 


Estimated  Distribution  of  SPA  Centers 


• — Previous  Filtered  Position 


Figure  3:  ModiHed  Filter  Operation  Geometry. 

A.  PROPOSED  MODinCATIONS 

Implementation  of  these  changes  in  the  code  is  a  part  of  a  separate  effort  to  port 
ASSET  to  the  Sun  UNIX  workstation.  A  listing  of  the  applicable  code  is  contained  in 
Appendix  B.  The  update-track  object  described  in  Chapter  III  ia  the  primary  object 
to  be  modified  to  implement  these  changes.  Its  function  can  be  split  between  those 
used  to  precalculate  the  variance  values  and  those  used  to  estimate  the  velocity  if 
precalculation  is  desired.  This  involves  computing  the  three  steady  state  variance 
values  for  each  sensor  AOU  size,  for  representative  values  of  the  contact  interarrival 
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time.  Other  objects  to  be  changed  are  mentioned  below  and  included  in  the  Appendix 
B  as  well.  A  summary  of  these  modifications  include: 

•  ioumotion:  The  three  element  values  of  the  f  matrix  (f(0,0),  f(0,2)  and  f(2,2)) 
and  the  two  ^  matrix  values  (^(2,2)  and  ^(0,2))  are  computed  as  before,  but  not 
set  in  matrix  form.  The  values  are  used  to  compute  the  three  element  values  of 
the  predicted  variance  using  Equation  (3.6).  The  three  computed  values  will  be 
used  to  compute  the  steady-state  filtered  variance  values  which  are  stored  in  a 
table  for  future  reference.  A  table  of  the  number  of  iterations  needed  to  reach 
steady  state  versus  interarrival  time  is  detailed  in  Appendix  C. 

•  change-tangent:  This  object  is  not  required. 

•  make-state:  The  random  numbers  used  to  determine  the  contact’s  latitude  and 
longitude  (from  the  contactPositlonDraw  function  called  by  the  Detection 
Reporter)  can  be  retained,  but  as  discussed  above  this  seems  unnecessary.  The 
two  values  of  the  R  matrix  are  all  that  need  be  calculated  by  this  object. 

•  filter:  The  values  computed  in  ioumotion  are  used  to  calculate  the  three 
elements  of  the  filtered  variance  using  Equation  3.10  for  representative  values 
of  the  contact  report  interarrival  time.  A  graph  of  the  steady  state  gain  Ka 
versus  interarrival  time  is  contained  in  Appendix  D.  The  Kalman  gains  are 
computed  in  order  to  find  those  values  and  will  be  used  below  to  set  the  range 
of  operation  of  the  filter. 


This  set  of  routines  comprise  the  pre-processing  module  and  can  be  separated 
from  the  rest  of  MTST  so  it  computes  the  necessary  data  at  the  start  of  the 
simulation.  This  requires  computing  the  three  variance  values  for  33  interarrival 
times,  giving  approximately  .03  between  succesive  table  values  from  .03  to  .97. 

This  requires  100  values  be  tabulated  for  each  AOU  defined.  Retrieving  values  from 
a  data  array  of  perhaps  1500  elements  appears  to  be  significantly  faster  than 
calculating  the  values  individually  each  time  the  filter  is  called. 
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The  following  routines  perform  the  necessary  computations  at  the  time  the  filter 


operation  is  performed; 


•  center-tangent:  This  object  is  modified  to  call  the  function 

contactPositionDraw  and,  using  the  filtered  variance  drawn  from  the  table 
computed  above,  as  positional  variance  choose  a  center  for  the  SPA.  The 
distances  involved  to  not  require  the  use  of  the  spherical  earth  routine  in 
contactPositionDraw.  The  new  latitude  and  longitude  can  be  calculated  using 
1®  of  latitude = 60. 1077nm  and  1®  of  longitude=60.1077*cos(latitude)nm.  The 
error  in  this  estimate  is  less  than  one  percent  out  to  ranges  of  600nm  at  non¬ 
polar  latitudes.  The  filtered  velocity  can  be  estimated  by  the  following 
equation: 


'^,.1  =  Vy,e-<-{\-IG)*K3ZVy, 

=  yx,e-“‘{\-K3)*K3ZVx, 

•  update-track-head:  The  position  and  velocity  computed  in  center  tangent  and 
the  new  variance  values  are  used  as  before  to  update  the  track-head.  The  state 
consists  of  the  position  latitude  and  longitude  and  the  corresponding  velocity 
components,  wMle  the  covariance  consists  of  the  three  steady-state  values  of  the 
filtered  variance. 

•  update-track-tail:  This  is  modified  in  the  same  way  as  update-track-head  to 
account  for  the  new  form  of  the  state  and  covariance. 


The  result  is  a  close  approximation  U>  the  variance  values  obtained  from  the 
full  implementation  and  a  position  that  represents  the  result  of  some  combination  of 
predicted  position  and  contact  position  that  was  not  specifically  identified  and  operated 
on.  The  distribution  of  estimated  filtered  states  closely  approximates  that  of  actually 
computed  states.  The  specifics  of  this  comparison  are  detailed  below. 
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B.  ACCURACY  OF  THE  MODHTED  TRACKER 


The  assumptions  underlying  this  new  procedure  introduce  deviations  from  the 
full  implementations.  As  discussed  above,  the  error  in  the  positioning  of  the  SPA 
about  the  target’s  true  position  error  is  maximum  for  Ka  approximately  .5-. 65.  The 
mean  value  of  this  error,  assuming  the  worst  case  of  a  non-maneuvering  target,  can 
be  calculated  using  Equation  (3.3)  to  find  distance  from  the  old  filtered  position  to  the 
new  observation  position.  Since  the  observations  are  centered  on  the  true  target 
position,  the  mean  of  this  value  can  be  calculated  using  the  true  target  position.  The 
results  are  shown  in  Appendix  E  for  favorable  and  unfavorable  conditions.  From 
these  results  it  can  be  seen  that  the  error  is  excessive  for  targets  at  high  speed  if  the 
time  between  detections  is  very  long.  This  quickly  drops  off  however,  and  for  more 
reasonable  time  intervals  of  less  than  one  hour,  the  relative  error,  even  for  high  speed 
targets,  is  negligible. 

Assumption  three  must  also  be  valid  for  the  approximation  to  be  reasonable. 

The  validity  of  the  first  part  of  the  assumption  is  shown  above.  This  requires  that  the 
variance  of  the  distribution  of  filtered  positions  be  equal  to  the  filtered  variance. 
Appendix  F  presents  a  comparison  of  filtered  position  distributions  and  filtered  AOU 
sizes  for  various  target  speeds,  computed  using  the  model  in  the  Appendix  G.  The 
data  in  Appendix  F  is  the  result  of  modelling  48  hours  worth  of  tracking  data  or  no 
more  than  300  observations.  The  simulated  target  conducted  a  random  tour  consisting 
of  ten  course  changes  executed  at  time  intervals  with  an  exponential  distribution  and  a 
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mean  of  4  hours.  The  target’s  speed  was  either  3  knots,  10  knots  or  30  knots.  The 
standard  ASSET  lOU  parameters  ^=.25  and  a=10j3‘'^  were  used.  The  results  of  30 
different  iterations  were  averaged  to  determine  the  mean  values. 

The  results  indicate  that  for  long  time  intervals  between  contacts,  and  speeds 
near  the  modeled  one,  the  results  are  fairly  good.  Excessive  deviations  are  exhibited 
when  the  target’s  speed  is  far  in  excess  of  the  modeled  speed.  Even  in  the  case 
where  the  actual  target  speed  was  ten  knots,  the  damping  of  the  velocity  is  apparent  in 
the  filtered  mean  speed.  When  this  is  combined  with  the  offset  error  described  above, 
errors  of  up  to  sixteen  percent  can  result.  While  this  is  a  large  error,  the  average 
value  this  error  takes  through  the  range  of  common  time  intervals  between 
observations  (.1  hours  to  1  hour)  and  possible  contact  speeds,  is  closer  to  five 
percent. 

One  source  of  this  error  can  be  seen  by  comparing  (3.10)  and  (3.12).  While  the 
single  Kalman  gain  Ka  is  used  to  compute  the  value  of  the  variance,  the  actual 
distribution  is  computed  using  both  Ka  and  Kbl.  For  the  range  of  AOU  sizes 
associated  with  submarines  and  MPA  assets  (at  worst  2Snm)  leaving  the  SPA 
unadjusted  amounts  to  an  error  in  standard  deviation  of  the  estimated  distribution  of 
less  than  Inm.  The  error  is  typically  conservative,  increasing  the  SPA  size  by  the 
error,  and  is  largest  when  the  time  interval  between  contacts  is  small.  This  produces 
errors  which,  while  a  significant  percentage  of  the  AOU  size,  are  actually  quite  small. 
A  notable  exception  is  the  decrease  in  the  standard  deviation  in  situations  where  the 
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target  speed  is  high,  counteracting  some  of  the  lOU  process’  tendency  to 
underestimate  the  targets  speed  in  this  case. 

The  failure  of  the  estimate  to  account  for  the  target’s  velocity  causes  the 
resultant  track  to  be  erratic.  This  effect  worsens  as  the  distance  traveled  between 
observations  gets  small  with  respect  to  the  AOU  size.  The  actual  filter  exhibits 
consistant  errors  in  this  case,  while  the  estimate  zig-zags  wildly  around  the  true  target 
track.  Since  the  linearity  of  the  track  is  not  critical  to  any  part  of  ASSET  except 
possibly  the  correlator,  the  effect  is  minimal.  For  any  single  point  the  estimate 
produces,  a  track  could  exist  which  includes  it.  The  random  draw  combines  elements 
from  all  these  hypothetical  tracks  into  a  single  collection  which  has  the  proper 
pointwise  relationship  to  the  real  track.  Thus,  while  specific  combinations  of 
observation  AOU  size  and  observation  interval  result  in  poor  performance,  others 
result  in  improved  performance. 

The  performance  of  the  modified  filter  is  reasonably  consistent  with  the  original 
filter  operation.  A  3-S  nm  error  in  a  25  nm  AOU  does  not  significantly  impact  the 
cueing  or  search  efforts  of  prosecuting  platforms  and  tracker  performance  is  actually 
improved  for  cases  where  the  target  is  transiting,  moving  at  high  speeds  without 
maneuvering.  The  errors  present  in  the  modified  tracker  appear  to  have  no  negative 
impact  on  the  operation  of  the  rest  of  the  simulation  and  while  introducing  moderate 
errors  in  tracking  maneuvering  targets,  balances  that  with  improved  performance 
against  non-maneuvering  ones.  Appendix  G  contains  examples  of  the  original  filter’s 
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tracking  ability  and  that  of  the  estimate  for  a  variety  of  sensor  AOU  sizes  and  mean 
interarrival  times. 

C.  LIMITING  THE  RANGE  OF  FILTER  OPERATION 

An  interesting  effect  apparent  from  Appendix  F,  is  the  limiting  nature  on  the 
filtered  position  distribution  of  the  contact  AOU  on  the  one  hand  and  the  predicted 
AOU  on  the  other.  This  situation  stems  from  the  same  effect  that  moves  the  filtered 
position  from  the  observation  position  toward  the  predicted  position.  This  can  be 
seen  directly  by  looking  at  the  values  of  the  Kalman  gains  as  a  function  of  range  for  a 
given  time  interval  between  observations.  Examples  of  these  relationships  are  shown 
in  Appendix  H.  As  can  be  seen  from  these  graphs,  if  the  observation  AOU  radius  in 
nautical  miles  is  limited  to  being  less  than  ISO  times  the  time  interval  in  hours,  the 
Ka  value  remains  above  the  "flat"  portion  of  the  curves,  approaching  zero. 
Observations  which  arrive  at  the  tracker  such  that  the  interval  since  the  last  contact  is 
too  small  compared  to  the  AOU  size  will  be  filtered  using  primarily  the  prediction 
information.  The  effect  of  the  contact  is  largely  ignored.  This  is  the  filter’s  way  of 
saying  that  the  information  value  of  the  contact  is  not  high  enough  to  be  considered  of 
value  in  computing  the  filtered  position. 

Such  contacts  can  be  ignored  as  there  is  little  difference  between  the  predicted 
position  and  variance  and  the  resultant  state  and  variance.  A  lower  limit  of  Inm  on 
the  observation  is  hardwired  into  the  make-state  and  compute-mean-cov  objects.  If 
the  time  between  contacts  is  too  short,  little  is  gained  unless  the  AOU  is  tiny.  The 
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Inm  limit  implies  that  the  minimum  time  interval  between  contacts  is  around  .01 
hours  to  allow  the  contact  data  to  be  assimilated  into  the  track.  If  the  contact  makes 
no  contribution  to  the  track  update,  there  is  no  reason  to  perform  the  filter  operation. 
The  effect  of  this  can  be  seen  in  the  data  in  Appendix  F,  where  the  predicted  and 
filtered  values  are  very  close  for  small  time  intervals  and  large  observation  AOUs. 
Thus  in  dense  contact  environments,  with  contacts  arriving  at  the  fusion  center  at 
intervals  of  less  than  30  minutes,  any  contact  based  on  an  AOU  larger  than  150  times 
the  time  since  the  last  contact  in  hours,  can  be  ignored  and  not  filtered.  This  value  of 
150  corresponds  to  a  Kalman  Gain  limit  of  approximately  .1.  The  exact  value  is 
rather  arbitrary  so  long  as  it  not  too  far  into  the  portion  of  the  curves  that  change 
quickly  with  variation  in  observed  AOU  size.  This  could  be  made  a  user  input  to 
limit  the  resolution  of  the  tracker  and  speed  execution  time  during  scenario 
development  and  testing,  and  then  reduced  to  provide  more  accurate  track  information 
when  the  data  is  actually  collected. 

In  similar  fashion  tracks  which  have  not  been  updated  in  a  long  time,  do  not 
contain  enough  information  to  affect  contacts  that  arrive  with  AOUs  less  than  5-10 
times  the  time  interval  since  the  last  observation.  If  the  value  of  Ka  is  very  close  to 
the  "flat"  portion  approaching  one,  the  predicted  value  is  ignored  in  favor  of  the 
contact  data.  This  allows  similar  saving  of  filtering  operations  in  sparse  track 
conditions  with  accurate  sensors.  If  the  time  interval  between  contacts  is  expected  to 
be  greater  than  one  hour  and  the  contact  AOUs  are  small,  using  the  raw  contact  to 
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update  the  track  is  a  reasonable  approximation  if  the  observation  AOU  size  is  smaller 
than  five  times  the  time  between  contacts  in  hours. 

When  these  two  "gating"  techniques  are  utilized,  substantial  numbers  of  filtering 
calls  can  be  eliminated.  This  can  be  particularly  true  in  scenarios  covering  large 
ocean  areas  with  many  possible  targets.  During  the  initial  stages  of  the  campaign,  as 
search  assets  slowly  get  cues  for  very  quiet  enemy  tactical  platforms,  detections  will 
not  be  made  until  the  enemy  is  at  close  range.  This  will  result  in  observations  with 
relatively  small  AOUs  coming  in  at  long  time  intervals.  As  cues  are  dispatched  and 
friendly  forces  converge  on  the  enemy,  even  a  modest  number  of  platforms  soon 
begin  producing  contact  streams  that  arrive  at  rapidly  decreasing  intervals.  Putting 
limits  on  when  the  filter  is  invoked  will  prevent  the  simulation  from  wasting  efforts 
computing  a  SPA  that  does  not  really  use  much  of  the  contact  information. 
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V.  CONCLUSION 


From  a  discussion  of  the  construction  and  use  of  the  Kalman  filter  based 
Maneuvering  Target  Statistical  Tracker  it  has  been  shown  that  the  tracker 
implementation  in  ASSET  is  not  currently  matched  well  to  the  types  of  inputs  it 
receives.  This  results  in  the  calculation  of  many  duplicate  values,  the  performance  of 
unnecessary  matrix  manipulations  and  the  calculation  of  values  in  situations  when 
little  useful  information  is  gained.  Improvements  to  the  tracker  aimed  at  decreasing 
the  computation  time  which  were  discussed  include: 


•  Use  of  the  equations  detailed  in  (3.3)  through  (3.12)  to  compute  the  actual 
filtered  variance  and  position. 

•  Filter  the  variance  of  the  contact  distribution  about  the  target  prior  to  conducting 
a  random  draw,  thus  producing  a  position  drawn  from  a  distribution 
approximating  that  of  the  actual  filtered  positions  variance.  Since  the 
observations  play  no  part  in  calculating  this  variance,  appropriate  values  for  the 
sensor  AOUs  can  be  precomputed  for  a  range  of  expected  time  intervals. 

•  Limit  the  situations  where  the  filter  is  ^tually  used  to  those  which  will  result  in 
a  meaningful  amount  of  information  being  extracted. 

•  Using  planar  estimations  of  the  latitude  and  longitude  computations  rather  than 
perform  the  conversions  on  a  "spherical  earth”. 


When  taken  together  these  modifications  greatly  reduce  the  complexity  of  the 
calculations  required  to  update  a  track  after  an  observation.  They  can  also  be  put 
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under  user  control  so  the  level  of  fidelity  can  be  adjusted  based  on  the  requirements 
of  the  architecture  and  the  stage  in  development  of  the  architecture  the  analyst  is  at. 
Areas  for  future  work  in  this  area  include: 

•  Formulating  a  faster  way  of  estimating  the  steady  state  gains  for  use  in  a  table 
of  pre-calculated  filtered  variances. 

•  Analysis  of  the  filtered  position  distributions  of  other  types  of  trackers  to 
determine  if  a  better  method  of  approximation  can  be  determined,  one  which 
better  accounts  for  target  velocity. 

•  Formulating  a  simple,  recursive  estimate  of  the  Kalman  gains  for  on-the-fly 
calculation.  An  attempt  at  this  by  using  limiting  values  as  the  time  interval  got 
small  yielded  very  good  steady  state  results,  but  divergence  and  chaotic 
behavior  for  small  AOU  sizes  ( <  lOnm  radius)  precluded  its  use  iteratively. 

•  An  analysis  of  the  algorithms  in  the  correlator  to  determine  if  all  of  its 
algorithms  are  necessary,  or  if  a  simple  probability  of  correct/false/no 
correlation  could  be  substituted.  If  appropriate,  determine  what  those 
probabilities  should  be. 

•  Modelling  how  the  AOU  size  should  change  with  respect  to  range  and  bearing 
from  the  searcher.  A  strict  linear  bearing  error  vs  range  is  a  simple  way  of 
solving  the  problem  of  constant  sensor  AOU  size.  The  real  situation  is  not 
quite  so  simple.  This  requires  computation  of  the  beam  sizes  and  beam 
distribution  of  a  towed  array  and  the  effect  of  TMA  techniques  on  determining 
the  position  course  and  speed  of  the  target.  This  would  allow  for  proper 
calculation  and  filtering  of  the  sensor’s  variance  in  velocity  measurement  as 
well. 

The  last  example  opens  up  a  plethora  of  possibilities  for  analysis  of  the  parameters 
required  for  input  to  ASSET.  A  compendium  of  data  on  everything  from 
communication  network  modelling,  to  aircraft  maintenance  data  analysis,  to  platform 
on  platform  engagement  simulation,  and  so  on.  Well  researched  values  for  the 
present  and  expected  future  interactions  of  platforms  and  their  supporting 
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infrastructures  need  to  be  produced  if  ASSET  and  simulations  like  it  are  to  be 
available  to  the  average  user. 

ASSET  has  the  potential  to  be  a  very  effective  analysis  tool.  A  great  deal  of 
work  must  be  expended  to  provide  the  necessary  data  to  allow  its  practical  use.  In  its 
present  form  it  requires  so  much  expert  knowledge  on  so  diverse  a  collection  of 
topics,  it  is  doubtful  one  person  could  perform  meaningful  analysis  without  an 
inordinate  amount  of  time  spent  in  research.  Continued  evolution  in  the  functionality 
of  the  simulation  will  provide  increased  applicability  and  accuracy  of  results. 

Increased  streamlining  of  the  computational  algorithms  will  be  necessary  as  this 
occurs  to  keep  it  running  on  a  desktop  workstation  in  a  reasonable  period  of  time. 
Striving  for  improvement  in  performance  however,  without  providing  a  ready  source 
of  data  will  most  probably  preclude  widespread  use  of  the  simulation. 
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APPENDIX  A:  CHARACTERIZATION  AND  ESTIMATION  OF  ^  AND  Q. 


The  following  equations  represent  the  elemets  in  the  transition  matrix,  phi,  and  the  lOU  process  noise,  Q. 
Approximation  equations  which  can  be  used  to  more  quicly  calculate  the  values  are  also  given. 
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The  graphs  which  follow  show  the  behavior  of  these  functions  over  various  time  intervals.  The  variable  "time" 
is  equal  to  the  elapsed  time  from  the  last  contact's  time  of  observation  to  that  of  the  present  contact,  in  hours. 
The  estimated  values  are  computed  using  the  first  two  significant  terms  in  the  series  expansion  of  the  exponential 
terms.  I'hese  estimates  are  very  accurate  for  mterarrival  times  of  two  hours  or  less. 
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APPENDIX  B;  APPLICABLE  SOURCE  CODE 


This  appendix  contains  portions  of  the  ASSET  source  code  which  would  be  modified 
to  implement  the  changes  described  above.  The  remarks  indicate  the  appropriate 
portions  which  would  be  deleted  or  changed.  This  is  not  an  all  inclusive  listing,  but 
contains  the  portions  most  affected  by  the  modifications. 


ACT4<TST 


«Md,A»r2S,lMe 


1 


OBJECT  ACT-MTST 

ACT-KTST  11  «n  Object  LISP  Inpliaentatlon  of  MIST,  the 
Hinueverln?  Tar^it  Statlitlcal  Tracker. 


(setq  act-ntit  (klndof  nil)) 


(tlafobfun  (axlit  act-«tatl  (Inlt-lltt) 

(usual -exist  inlt-llst) 

(Mve  'dtor  (/  pi  180)) 

(have  ‘alpha  0.2S) 

(have  'slqna  (sqrt  (/  100  2)))  ;  10  knot  target 


(dafobfun  (create-ob ject-llnks  aet-ntst)  (oals) 
(have  '  IdbB  (ask  oals  lAa) ) 

) 


(defobfun  (itart-ne«-track  act-srtat)  (uc  track-id) 

(let*  ( (mean-cov-llat  (conpute-aean-eovarlance  uo) 

(aean  (first  Man-cov-llst) ) 

(var  (second  aean-cov-liat) ) 

(alt  (obu-contact-altltuda  uc) ) 
tuc-apatlal-datk  (obu-contact-apatial-data  uc) ) 

(lat  (obu-data-fleld-lat  uc-spstlal-data) ) 

(Ing  (obu-data-fleld-lnq  uc-ipatlai-data) ) 

(ctc-liit  (Hat  UO) 
track  state-cov 

) 

(setq  state-cov  (aake-obu-state-field 

: ref -lat  lat  ;ref-lnq  Inq 
; state  aean  : covariance  var)) 

(return- from  itart-ne«-traek 

(aaka-obu-track  ;ld  track-id 

•nanber-of -contacts  1 
: head-ststs-covarlancc  state-cov 
; head-contact -Id  (obu-contact-ld  uo 
:head-apatlal-data  ue-apatlal-data 
: tall -atate-covar lance  state-cov 
:tall-contact-ld  (obu-contact-ld  uo 
: tall-spatial -data  uc-spatlal-data 
:  altitude  alt 
•contacts  ctc-llst 

) 
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ACT-MTST 


Wad.  Apf  25, 1M0 


2 


(defobfun  (conpute-mean-covarlance  act-«tsti  (uc) 

(let*  ( (uc-spatlal-data  (obu-contact-spatlal-daca  uc) ) 
(type  (obu-data-f leld-typc  uc*spatlal*data) ) 
(lat  (obu-data*field-lat  uc-spatlal-data) ) 
(Ing  (obu-data-fleld-lnq 
(brq  (obu-data-tield-brq 
(maj  (obu-data-fleld-na j 
(min  (obu-data*(leld-mln 
(cse  (obu-data-fleld-cse 


uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-spatlal-data) ) 
uc-spatlal-data) ) 


(cse-unc  (obu-data-fleld-cse-unc  uc-spatlal-data) ) 
(spd  (obu-data-fleld-spd  uc-spatlal-data)) 

(spd-unc  (obu-data-fleld-spd-unc  uc-spatlal-data)) 
(mean  (maJ(e-airay  4  ;  Initial -element  0)) 

(vax  (maJce-atray  ‘(4  4)  ;  Initial -element  0)) 
stationary  saaj  smln  cosb  sinb  cosb2  slnb2 


sl^sa  sl9u)  (•  2  alpha) ) ) 


(*  dtor 
(*  dtor 


90  cse) ) ) ) ) 
90  cse) ) ) ) ) 


) 

(setq  stationary  (/ 

(setf  (aref  mean  2) 

(setf  (aref  mean  3) 

(setf  (aref  var  2  2) 

(setf  (aref  var 
(setq  smaj  (max 
(setq  smln  (max 
(setq  cosb  (cos 
(setq  slnb  (sin 
(setq  cosb2  ( 

(setq  slnb2  ( 

(setq  smaj2  (•  0.25  sma}  sma])) 

(setq  smin2  (*  0.25  smln  smln)) 

(setf  (aref  var  0  0)  (*  (*  smin2  cosb2) 

(setf  (aref  var  0  1)  <•  <-  sma*2  aBiln2) 

(setf  (aref  var  1  0)  (aref  var  0  1)) 

(setf  (aref  var  1  1)  (^  (*  smin2  Slnb2)  (*  sma]2  cosb2))) 

(teturn-from  coB.pute-mean-covariance  (list  mean  var)) 


( 

(•  spd  (cos 
(*  s^  (sin 
stationary) 
3  3)  stationary) 
ma]  1) ) 
min  1) ) 

(•  dtor  brq) ) ) 

(•  dtor  brq) ) ) 
cosb  cosb) ) 
slnb  slnb) ) 


(*  ama)2  slnb2) ) ) 
sinb  cosb) ) 


(defobfun  (update-trac)c  act-mtst)  (uc  traOc) 

(let*  ((uc-spatlal-data  (obu-contact-spetlal-data  uo ) 

(Id  (obu-contact-id  uo) 

(type  (obu-data-f leld-type  uc-spatlal-data) ) 

(lat  (obu-data-f leld-lat  uc-spatlal-data)) 

(Inq  (obu-data-fleld-lnq  uc-s^tlal-data) ) 

(brq  (obu-data-f  isld-brq  uc-^atlal-data) ) 

(ma3  (obu-data-fleld-maj  uc-^atlal-data) ) 

(min  (obu-data-fleld-mln  uc-s^tlal-data) ) 

(cse  (obu-data-fleld-cse  uc-spatlal-data) ) 

(cse-unc  (obu-data-flcld-cse-unc  uc-spatlal-data) ) 
(spd  (obu-data-fleld-spd  uc-spatlal-data)) 

(spd-unc  (obu-data-fleld-spd-unc  uc-spatial-datal ) 
(obs-tlme  (obu-data-fleld-obs-tlne  uc-spatlal-data)) 
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(track-spatlAl-dAta  (obu-track-haad'Spatial-data  track) ) 
(tall-apatlal-data  (obu-crack-cail-spatlal-data  track)) 

( last - update -t la*  (obu-data-fleld-obs-tlae  track-spatial -data) ) 
(tail-update-tiae  (obu-data-field-obs-tlae  tail-spatial-data  i i 
(last-state-cov  (obu-track-head-statc-covarlance  track)) 
(tali-atate-eov  (obu-track-tail-state-covarlance  track)) 
(laat-ref-lat  (obu-state-flald-ref-lat  last-state-cov)) 
(last-ref-lng  (obu-state-fteld-ref-lng  last-state-cov)) 
state-cov-old  state-cov-ctc  state-cov-new  delta-tlae 
contacts  tlae-old  aean-var-list  pos  etc  ctc-sd  count 


(setq  delta-tlae  (-  obs-tlae  last-update-tlae) ) 

(If  (>-  delta-tlae  0) 

(progn 

(setq  state-cov-old  (change-tangent  lat  Ing 

(loiaotlon  delta-tlae  last-state-cov))) 

(setq  state-cov-ctc  (aaka-stat*  type  lat  Ing  brg  aa]  min  cse  cse-unc 
spd  spd-unc) ) 

(setq  stat*-cov-n*«  (center-tangent  (filter  state-cov-old 
state-cov-ctc) ) ) 

(setq  track  (update-track-head  track  uc  state-cov-new 
uc-spatial-data  id)) 

(setq  track  (update-track-tall  track)) 

) 

(progn 

(if  (or  (>  obs-tlae  tall-update-tlaa) 

(<  (obu-track-ntsBber-of-contacts  track) 

(ask  Idbei  contacts-frem-haad-to-tall) ) ) 

(progn 

(setq  contacts  (append  (obu-track-contacts  track)  (list  uoi) 
(setq  contacts  (sort  contact*  ** (lambda  (x  y) 

(<  (obu-data-fi*ld-obS''tlae  (obu-eontact-spatiai-data  x() 
(obu-data-f lald-obs'tlma  (obu-contact -spat i a 1 -data 

>■11)1)1 

(if  (>-  obs-tlm*  tail-upd*te-tl*») 

(progn 

(setq  state-cov-old  tall-state-cov  ) 

(setq  time-old  tall-(q>date-tlme) 


contacts) ) ) 


(first  contacts)))) 
1  first  contacts) ) ) ) 


(progn 

(setq  siean-var-llst  (compute-mean-covarlance  (first 

^setq  lat  (obu-data-field-lat  (obu-contact-spatial-data 

(setq  Ing  (obu-data-fleld-lng  (obu-contact -spatial -data 

(setq  state-cov-old  (make-obu-state-f leld 
;r*f-lat  lat 
:r*f-lng  Ing 

: state  (first  mean-var-l) st) 

:covarl*nce  (second  mean-var-llsti ) ) 

(setq  tiaie-old  (obu-data-flald-obs-tlme 

(obu-contact-spatlal-data  (first 
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(setq  count  (min  (ask  Idbni  contacts-from-head-to-tall I 
(-  (list-langth  contacts)  111) 

(dotunas  (1  count  nil) 

(satq  pos  (»  (oftu-track-nu«bar-of-contacts  track)  (-  count)  1 

(satq  etc  (ntn  pos  contacts) I 

(satq  ctc-sd  (obu-contact-spatlal-data  etc)) 

(satq  typa  (obu-data-fiald-typa  ctc-sd) ) 

(satq  obs-tiaa  (obu-data-flald-obs-tlme  ctc-sd) I 
(satq  lat  (obu-data-flald-lat  ctc-sd) I 

(satq  Ing  (obu-data-flald-lnq  ctc-sd) ) 

(satq  brq  (obu-data-flald-brq  ctc-sd) I 

(satq  na]  (obu-data-flald-iBa]  ctc-sd)  I 

(satq  min  (obu-data-flald-mln  ctc-sd) ) 

(satq  esa  (obu-data-flald-cse  ctc-sd) ) 

(satq  cse-unc  (obu-data-flald-csa-unc  ctc-sd) ) 

(satq  spd  (obu-data-*lald-spd  ctc-sd) ) 

(satq  spd-unc  (obu-data-fiald-spd-unc  ctc-sdi ) 

(satq  dalta-tusa  (-  obs-tlna  tlaa-oidl ) 

(satq  stata-cov-old  (chanqa-tanqant  lat  Inq 

(loisBotlon  delta-time  stata-cov-old) i ) 
(satq  state-cov-etc  (make-stata  type  lat  Inq  brq  ma:  mir.  cse 


(last  contacts))) 
contacts) I ) ) ) 

) 


spd  spd-unc) ) 

(satq  stata-cov-old  (center-tanqent 

(filter  stata-cov-old  state-cov-ctc) 1 1 
(satq  tima-old  obs-timel 

I 

(satq  track  (updata-track-haad  track  uc  stata-cov-old 

(obu-contact-spatiai-data  (first 

(obu-contact-id  (first  (last 

(satq  track  (updata-traek-tall  track)) 


(return-from  updata-track  track) 

I 

I 


(defobfun  (updata-track-haad  act-mtst)  (track  uc  state-cov  spatial-data  idi 
(.at  ((contacts  (append  (obu-track-contacts  track)  (list  uc) ) ) 

) 

(satq  contacts  (sort  contacts  ('(laadsda  (x  y) 

(<  (obu-data-flald-obs-tlme  (obu-contact-spatlal-data  xii 
(obu-data-flald-obs-tusa  (obu-contact-spatlal-data 

yi ) 1 1 )  1 

(setf  (obu-track-nissbar-of-contacts  track)  (♦  (obu-track-natiber-of-contscts 
track)  1 1 1 

serf  (obu-track-haad-state-covartance  track)  stste-cov  | 
isetf  (otu-track-haad-spatial-data  track)  spatial -data) 
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(setf  (obu-tr«clc-h««d-cont«ct-i<l  tr«ck)  Id  ) 
(setf  (obu-track-contacts  track)  contacts  ) 
(ret-rn-Jrom  update-track-haad  track) 


(dafotfun  (updata-track-tall  aet-mtst)  (track) 

(let*  ( (nctcs  (obu-track-ntanbar-o(-contacts  track) ) 

(haad-sc  (obu-track-haad-stata-covarlanca  track)) 

(haad-sd  (obu-track-haad-spatlal-data  track) ) 
lhaad-id  (obu-track-haad-contact-ld  track) ) 

(tall-ac  (obu-track-tall-state-covarlanca  track)) 

(tail-sd  (obu-track-tall-spatial-data  track)) 

(tail-ld  (obu-track-tali-contact-ld  track)) 

(tall-updata-ttaa  (obu-data-flald-obs-tlaa  tall-ad) ) 
(contacts  (obu-track-contacti  track)) 

typa  obs-tina  lat  Ing  brg  aa]  nln  csa  csa>unc  spd  spd-unc 
maan-var-llat  naw-tail  pos-naw-tall  nax-tall-sd  dalta-tioa 
stata-cov-old  stata-cov-ctc  itata-cov-naw 


y  I )  1 1 1 


I 

(satq  contacts 


(sort  contacts  ('(laabda  (x  y) 

(<  (obu-data-flald-obs-tlBa  (obu-contact-spatlai-data  x) ) 
( obu-data-  f  tald-^t-t  laa  ( obu-contact  -  spat  t  al  -dat  a 


I 


(If  (<-  nctcs  1) 

(pro<}n  (satf  (obu-track-tall-stata-eovarianct  track)  haad-sc) 

(satf  (obu-crack-cail-spattai'daca  track)  baad-sd) 

(satf  (obu-track-tall-contact'ld  track)  haad-ld) 

(raturn-frcn  updata-track'tall  track) 

I 


) 


(if  (<-  nctcs  (ask  Idbs)  contacts-frcn-)Mad'to-tall) ) 

(If  (-  tall-ld  (obu-(»ntact-td  (first  contacts))) 

(ratum-froai  updata-track-tall  track) 

(proqn 

(satq  Bwan-var-llst  (ceaq)uta-«aan-covarlanca  (first  contacts))) 
(satq  lat  (obu-data-flald-lat  (obu-contact-spatlal-data  (first 

contacts) ) I i 

(satq  Inq  (obu-data-fiald-lnq  (obu-contact-spatlal-data  (first 

contacts) i ) ) 

(satf  (obu-track-tall-stata-cowarlance  track)  (inaka-obu-state-field 
;raf-lat  lat 
(caf-lnq  Inq 

:stata  (first  isaan-var-llst) 

(covarlanca  (sacond  aear.-var-llst) ) ) 

(satf  (obu-track-tall-spatlal-data  track)  (obu-contact-spatlal-data 
(first  contacts) ) ) 

(satf  (obu-track-tail-contact-ld  track)  (obu-contact-ld  (first 

contacts) ' ) 

(return-froB!  updata-track-tall  track) 


) 
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(setq  pos-new-tall  (-  (obu-track-r\*jab«r-of«contacts  track) 

(ask  IdbTJ  contactS“f rom-head-to-tail ) > I 
(setq  new-tall  (nth  pos-new-tail  (obu-track-contacts  track))) 

(setq  new-taii-sd  (obu-contact-spaclal-data  new-tall) ) 

(setq  type  (obu-data-field-type  new-tail-ad) ) 

(setq  obs-tlAe  (obu-data-field-obs-tUae  new-tail-sd) ) 

(*setq  lat  (obu-data-field-lat  new-tall-sd) ) 

(setq  Inq  (obu-daca-fieid-lnq  new-tail-sdi ) 

(setq  brg  (obu-data-fleld-brg  new-tail-sd)) 

(setq  tnaj  (obu*data-fleld-«aj  Mw-tail-sd)) 

(setq  mm  (obu-data-fleld-oin  new-tall-sd)) 

(setq  cse  (obu*data-field-cse  new-tall-sd)) 

(setq  cse-unc  (obu-data-fieid-cse-unc  new-tail-sd) ) 

(setq  spd  (obu-data-fleld-spd  new-tall-sd)) 

(setq  spd-unc  (obu-data-field-spd-unc  new-tall-sd)) 

(setq  delta-time  (-  obs-tiae  tall-update-tlAt) ) 

(setq  state-cov-old  (change-tangent  lat  Ing  (loxssotlon  delta-time 
tail-sc) ) ) 

(setq  state-cov-ctc  (make-state  type  lat  Ing  brg  maj  min  cse  cse-unc  spd 

spd-ur.c) ) 

(setq  state-cov-new  (center-tangent  (filter  state-cov-old 
state-cov-ctc) ) ) 

(setf  (obu-track-tail-state-covariance  track)  state-cov-new) 

(setf  (obu-track-tail-spatial-data  track)  new-tall-sd  ) 

(set;  (obu-track-tail-contact-ld  track)  (obu-contact-id  new-tail) ) 

(prune-contacts-before-tall  track)  :  Get  rid  of  contacts  before  tail. 

(return-frotn  update-track-tall  track) 


(defotfur.  (prune-contacts-before-tall  act-mtst)  (track) 

(.et  t (nctcs  (obu-track-nvasber-of-contacts  track)) 
(contacts  (obu-track-contacts  track) ) 

(.ttax-ctcs  (ask  Idbn  contacts-from-head-to-tail) ) 
tail-position 

) 

(setq  tail-position  (-  nctcs  max-ctcs)) 

(if  (<-  tail-position  0) 

(return-fron  prune-contacts-before-tail  nil) 

) 

(dotimes  (i  tail-position  nil) 

(setq  contacts  (remove-elenent  contacts  0)) 

) 

(setf  (obu-track-noBber-of-contacts  track)  max-ctcs) 

(setf  (obu-track-contacts  track)  contacts) 


(defocfur.  (loumotion  act-mtst)  (time  state-cov) 
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(l«t  (dat  (obu-atata-fiald-raf-lat  ttatA-cov)) 

(Ing  (obu^state'fleld^rcf^lng  state-cov) ) 

(mean  (obu-state-fleld*state  state-cov) ) 

(vau^  <obu-state-f ield'covariance  state-cov) ) 

motlon-natrlcas  phi  f  new^taean  naw^var 
varta^  phitrn 

) 

df  (*  tlaa  0) 

(return-from  loianotlon  state-cov) 

) 

(setq  motion-fnatrlces  (caic-ftot-oat  tiaa)) 

(setq  phi  (first  cnotion-Aatrlces) ) 

(satq  f  (second  motion-tBatrices) ) 

(setq  new-fflean  (matrix-multlply  phi  mean)) 

(setq  vartmp  (natrix-muitlply  phi  var  )) 

(setq  phitrn  (matrix-transpose  phi)) 

(setq  new-var  (natrix-add  (matrix-iBuXtiply  vartop  phitrn)  f)) 

(setq  phi  new-var) 

(setq  phitrn  (matrix-transpose  new-var)) 

(setq  new-var  (matriK-scalar-multtply  0.5  (matrix-add  phi  phitrn))) 
(return-from  ioxsnotion  (maXe-obu-state-field 
:ref-lat  lat 
:ref-lnq  ing 
: state  new-mean 
: covariance  new-var) 


(defofcfun  (calc-mot -mat  act-mtst)  (time) 

(let*  ((invalpha  (/  I  alpha)) 

(invalpha2  (*  invalpha  invalpha)) 

(expalphat  (exponential  (*  -X  alpha  time))) 
(exp2alphat  (*  expalphat  expalphat)) 

(f  (ffia)ce-array  '(4  4)  :  initial -element  0)) 
(phi  (make-array  * (4  4)  : initial -element  0)) 


(if  (>  time 
(progn 


0) 


(setf 

(araf 

f 

0 

0) 

<•  lnvalpha2  (- 

time 

(.  invalpha 

(-  (• 

2 

(• 

•  1 

axpaiphatl  1  (*  . 

5  (- 

1  exp2alphat))U))) 

(setf 

(ar<( 

f 

1 

1) 

(arat  C  0  0) 1 

(setf 

(«ia( 

f 

0 

2) 

(*  inva3pha2  (* 

0.5 

1-  expalphat) 

(•  .5 

axp2alphac) 1 ) 1 

(setf 

(araf 

f 

2 

01 

(aref  f  0  2)) 

(setf 

(arar 

f 

1 

3) 

(aref  f  0  2)) 

(setf 

(araf 

f 

3 

1) 

(aref  f  0  2)) 

(setf 

(araf 

f 

2 

2) 

{*  0.5  invalpha 

(-  1 

exp2alphat) ) 1 

(setf 

(araf 

f 

3 

3) 

(aref  f  2  2)) 

(progn 

(setf 

(setf 

(setf 


(aref  f  0  0)  (•  invalpha2  (-  (-  time)  (*  Invalpha 
(-  (•  2  (-  expalphat  D)  (•  .5  (-  exp2alphat  1))))))) 
(aref  f  I  1)  (aref  f  0  0) ) 

(aref  f  0  2)  (*  lnvaipha2  (♦  (-  0.5)  expalphat 
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<setf  (aref  f  2  0)  (arcf  f  0  2)) 

(setf  (aref  f  1  3)  (aref  f  0  2)) 

<s«tf  (aref  f  3  1)  (aref  f  0  2)) 

(setf  (aref  f  2  2)  (*  O.S  invalpha  (*  exp2alphat  I))) 

(setf  (aref  f  3  3)  (aref  f  2  2)) 

) 

) 

(setq  f  (matrix-scalar-multiply  (•  siiyna  sigma)  f)) 

(setq  phi  (identity-matrix  4)) 

(setf  (aref  phi  0  2)  (•  invalpha  (-  1  expaiphat))) 

(setf  (aref  phi  1  3)  (aref  phi  0  2)) 

(setf  (aref  phi  2  2)  expaiphat) 

(setf  (aref  phi  3  3)  (aref  phi  2  2)) 

(teturn-from  calc-mot -mat  (list  phi  f)) 


(defotfun  (exponential  act-mtst)  (x) 

(if  l<  x  -350) 

(return-fron  exponential  (exp  -350)) 

1 

(if  (>  X  350) 

(return-fron  exponential  (exp  350)) 
(rct'orn-fran  exponential  (exp  x)) 

) 

) 


(defobfun  (make-state  act-mtst)  (type  lat  Inq  brg  ma)  min  cse  cse-unc  spd  spd-ur.o 
(let  ((mean  (make-array  4  :  Initial -element  0)) 

(var  (make-array  M4  4)  : initial -element  0)) 

(Stationary  (/  {*  siqma  si^oa)  (*  2  alpha))) 

(smaj  (max  ma^  D)  (mn  (max  min  1)) 

} 

(setf  (aref  mean  2)  (•  spd  (cos  (•  dtor  (-  90  cse))))) 

(setf  (aref  mean  3)  (*  (sin  (*  dtor  (-  9C  cse))))) 

(setq  var  (make-var  stationary  brg  sma^  smin) ) 

(return-from  make-state  (okake-obu-state-field 
:ref-lat  lat 
:ref-lng  Ing 
: state  mean 
: covariance  var)) 

) 

1 


(defcbf-jn  (make-var  act-mtst)  (stationary  brg  smaj  sr.in) 
(let*  ((var  (make-array  *(4  4)  : initial-element  0)) 
(cosb  (cos  (•  dtor  brg))) 
isinb  (sin  (*  dtor  brg))) 

(cosb2  (*  cosb  cosbi) 

(sinb2  (*  slab  sinb) ) 


72 


ACT4<rTST 


W«d.  Apr  25. 


{snaj2  (*  0.25  smAj  mij)) 

(Sffln2  (*  0.25  stfiln  smn)) 

i 

(se^f  (Aref  vax  0  0)  (-*^  (*  snun2  cosb2)  (*  sinaj2  5inb2))) 

(setf  {aref  var  C  1)  (*  (-  snaj2  SBU.n2)  sinb  cosf>) ) 

(setf  (aref  var  1  0)  (aref  var  0  D) 

(setf  (aref  var  1  1)  (*  srLin2  slnb2)  (*  smaj?  cosb2) ) ) 

(setf  (aref  var  2  2)  stationaiy) 

(setf  (aref  var  3  3)  stationary) 

(return-f ram  make-var  var) 

) 

) 


(defobfun  (filter  act-mtst)  (state-cov-sol  state-cov-cto 
(let  (dat  (obu-state-fieid-ref-lat  state-cov-sol)) 
(ing  (obu-state-field-ref-lng  state-cov-aol) ) 
(meanl  (obu-state-field-state  state-cov-sol) ) 

(varl  (obu-state-fleld-covariance  state-cov-sol)) 
(mean2  (obu-state-field-state  state-cov-cto ) 

(var2  (obu-state-fleld-covarlance  state -cov-ctc) ) 
(infmatl  (aiake-array  *(4  4))) 

(infmat2  (nake-array  '(4  4))) 

(infvecl  (make-array  4)) 

(infvec2  (fflake-array  4)) 

(citpinat  (make-array  '(4  4))) 

(tmpvec  (make-array  4)) 
mean  var 

) 

(setq  infmatl  (matrlK-invert  varl)) 

(setq  infvecl  (matriK-muitipiy  infmatl  meaniu 
I setq  infmat2  (matrik-invert  var2)) 

(setq  infvec2  (fflatrix-multiply  infmat2  nean2)) 

(setq  tnpoat  (matrlk-add  infmatl  Inf&at2)) 

(setq  var  (matrix-invert  tmpmat)) 

(setq  tmpvec  (matrix-add  infvecl  lnfvec2)) 

(setq  mean  (matrix-multiply  var  tmpvec)) 
(retum-from  filter  (make-obu-state-fleld 
:ref-lat  lat 
:ref-lng  inq 
(State  mean 
'.covariance  vax) 


(defobfun  (chanq-  tanqent  act-mtst)  (newlat  nevlnq  state-cov) 
(let  ( (oldlat  (obu-state-fleld-ref-lat  state-cov)) 
(oldlng  (obu-state-field-ref-lnq  state-cov) ) 

(mean  (obu-state-field-state  state-cov) ) 

(var  (obu-state-fleld-covariance  st3te-cov> ) 

(rot  (make-array  ’(4  4)  (initial-element  0)) 
(rottrn  (make-array  ’(4  4))) 

(neirtmp  (make-array  *  (4  4))) 
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(new-tnaan  (make-array  4  )) 

(neM-var  (make-array  '(4  4))) 

latinglist  rngtrgllst  cenlat  cenlng  (Seltabrg  cosb  sir^ 
rngl  rng2  rng3  brgl  brg2  brg3 

) 

(setq  mgl  (sqrt  (♦  (•  (aref  mean  0)  (aref  mean  0)) 

(*  (aref  mean  1)  (aref  mean  1)}))) 

(setq  brgl  (arctan  (aref  mean  1)  (aref  mean  0) n 
(setq  latinglist  (getlatlng  oldlat  oldlng  rngl  brgl  0)) 

(setq  cenlat  (first  latinglist)) 

(setq  cenlng  (second  latinglist)) 

(setq  brgl  (second  (getrngbrg  oldlat  oldlng  cenlat  cenlng  0)}) 

(setq  brg2  (second  (getrngbrg  cenlat  cenlng  oldlat  oldlng  QM) 

(setq  rn«^rgllst  (getrngbrg  newlat  newlng  cenlat  cenlng  0)) 

(setq  rng3  (first  rngbrgllst)) 

(setq  brg3  (second  rngbrgllst)) 

(setq  brg4  (second  (getrngbrg  cenlat  cenlng  netrlat  newlng  0))) 

(setq  deltabrg  (mod  (-»  (-  brg2  brgl.  (-  brg3  brg4)  360)  3€0|) 

(setq  cosb  (COS  (*  dtor  deltabrg))) 

(setq  sinb  (sin  (*  dtor  deltabrg))) 

Move  the  mean  position: 

(setf  (aref  new-mean  0)  (•  rng3  (cos  (•  dtor  (-  90  brg3))))) 

(secf  (aref  new-mean  1)  {•  rng3  (sin  (•  dtor  (-  90  brg3))))) 

Rotate  the  velocity: 

(setf  (aref  new-mean  2)  {*  (*  cosb  (aref  mean  2))  (*  sinb  (aref  mean  3)M) 

(setf  (aref  new-mean  3)  (-  (*  cosb  (aref  mean  3)}  (*  sinb  (aref  mean  2))i) 

Rotate  the  covariance: 

(setf  (aref  rot  0  0)  cosb) 

(setf  (aref  cot  I  11  cosb) 

(setf  (aref  rot  2  2)  cosb) 

(setf  (aref  rot  3  3)  cosb) 

(setf  (aref  rot  0  1)  sinb) 

(setf  (aref  rot  2  3)  sinb) 

(setf  (aref  rot  1  0)  (-  sinb)) 

(setf  (aref  rot  3  2)  (-  sinb)) 

(setq  newtittp  (matrix-multlply  rot  var)) 

(setq  rottrn  (matrix-transpose  rot)) 

(setq  new-vai  (matrix-multlply  newtmp  rottrn)) 

(setq  rot  new-var) 

(setq  rottrn  (matrix-transpose  new-var)) 

(setq  new-var  (matrix-scalar-multiply  0.5  (raatrlx-add  rot  rottrn))) 
(return- from  change-tangent  <maXe-obu-state-f leld 

:ref-lat  newiat 

:ref-lng  newlng 

: state  new-mean 

: covariance  new-var) 


f 
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(defobfun  (cantar-tanqant  act-atatl  (atata-cov) 

(l«t  (<lat  (obu-state-Jlald-raf-lat  itata-covii 
(Ing  (obu-state-dald-raf-lng  stata-cov) ) 

(maan  (obu-itata-flald'SCata  atata-covi | 

(var  (obu>atata-flald'covarlanca  atata-cov) ) 

(rot  (aaka-array  '(4  4)  : initial -alasant  0)) 

(rottrrv  imako-artay  '(4  4))) 

(nawtaip  (maka-array  '  (4  4) ) ) 

(naw-oaan  (maJca*array  4  )) 

(naw-var  (maJca-axray  '  (4  4) ) ) 

latlndlist  rnqbr^llat  na«lat  nawlng  nawrng  nawbrg 
rnql  bcqi  coat)  alnb 

) 

(aatq  rnql  (aqrt  («  (•  (araf  Man  01  (araf  Man  0)1 

(<  (araf  Man  1)  (axaf  Man  1))))) 

(If  (<  mql  0.01) 

(raturn-traai  cantar-tanqant  atata-cov) 

) 

(satq  Brql  (arctan  (araf  Man  1)  (araf  Man  0))) 

(satq  latlnqllat  (qatlatlnq  lat  inq  mql  ergl  0)) 

(satq  navlat  (flrat  latlnqllat!) 

(satq  nawlnq  (aacond  latlnqllat)) 

(satq  mqbrqllat  (qatmqbrq  navlat  nawlnq  lat  Inq  0)) 

(satq  nawmq  (flrat  m^rqllat)) 

(satq  nawbrq  (laod  (*  (aacond  mqbrqllat)  ISO)  360)) 
isatq  daltabrq  (-  nawbrq  brql)) 

(satq  cosb  (cos  (*  dtor  daltabrq))) 

(satq  slnb  (sin  (*  dtor  daltabrq))) 

MDva  tha  B«an  position: 

(satf  (araf  naw  Man  0)  0) 

(satf  (araf  naw-Man  1)  0) 

Rotata  tha  valoelty: 

(satf  (araf  naw  Man  2)  (*  (*  coab  (araf  Man  2) )  (*  slnb  (araf  Man  3)  I ) ) 

(satf  (araf  naw-Man  3)  (*  (*  cosb  (araf  Man  3))  ('  slnb  (araf  mean  2)))) 

Rotata  tha  covarlanca: 

(satf  (araf  rot  0  0)  coab) 

(satf  (araf  rot  1  1)  coab) 

(satf  (araf  rot  2  2)  cosb) 

(satf  (araf  rot  3  3)  coab) 

(satf  (araf  rot  0  1)  slnb) 

(satf  (araf  rot  2  3)  alnb) 

(satf  (araf  rot  1  0)  (-  alnb)) 

(satf  (araf  rot  3  2)  (-  alnb)) 

(satq  nawta^  (Mtrlx-aultlply  rot  var)) 

(satq  rottm  (Mtrlx-tranapoaa  rot)) 

(satq  nsw-var  (Mtrlx-«ultlply  nawt^  rottm)) 

(satq  rot  naw-var) 

(satq  rottrn  (Mtrlx-tranapoaa  nsw-var)) 

(satq  naw-var  (Mtrlx-scalar-aultlply  C.5  (Mtrlx-add  rot  rottm))) 
(raturn-froBi  cantar-tanqant  (aa)ta-^>u-stata-tlald 

:rsf'lat  navlat 
:taf-lnq  nawlnq 
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:  ttcc*  rmt-mman 

:cov»zitnct  n«w-varl 


(Oefsbiun  t»zct*n  act-atstl  (y  xl 
(let  (« 

I 

(If  (and  (-  y  Ql  (-  x  0) I 
(rtturn-frcR  atetan  0) 

) 

(If  (•  y  0( 

(progn  (If  «  x  01 

(ratum-frOB  arctan  2^0) 
(ratorn-froBi  arctan  90l 


(if  (<  y  0) 

(aatq  a  (♦  (/  (atan  (/  x  yl >  dtor)  180)1 
(aatq  a  (/  (atan  (/  x  y))  dtor)  ) 

) 

(If  (>  a  0) 

(raturn-frcB  arctan  ai 
(raturn-fro«  arctan  (»  a  3«0)) 

) 


This  prooadura  convarta  a  eovarlanen  aatrlx  (eov)  into  an 
allipta.  TTw  allipaa  ii  ratumad  in  tha  fon: 

(  Oriantatlon  ,  Saal-Bajor  axla  ,  Saal-adnor  axis  ) . 


(dafobfun  (cov-to>allipsa  aet-atst)  (covi 
(iat  ( (a  (araf  cov  0  Ol ) 

(b  (araf  eov  o  l)l 
(c  (araf  cov  l  D ) 

brq  coab  sinb  eosbJ  tinb2  tincosZb  aanr 

I 

(if  (not  (>  (-  (•  1  e)  (•  b  bl>  0)) 

(cacum-froa  cov>to-alllpss  (list  0  (axpt  10  9)  (axpt  10  9) ) ) 

) 

(if  (-  b  0) 

(If  (not  (>  a  c) ) 

(ratum-froa  eov'to-alllpsa  (list  0  (*  2  (sqit  (abs  c))) 

(*  2  (sqct  (abs  a) I ) ) ) 

(raturn-froB  cov-to-alllpsa  (list  90  (•  2  (sqrt  (abs  a))) 

(•  2  (sqrc  (abs  c) ) ) ) ) 


) 

) 

(if  (>  b  0) 

(if  (-  a  c) 

(ratum'froB  eovto-alllpsa  (list  48  (*  2  (sqrt  (•-  a  bi)l 

(•  2  (sqrt  (-  a  b) ) ) 1 ) 

) 

I 

(if  i<  b  0) 

(if  (-  a  c) 

(ratum-froa.  cov-to-alllpsa  (list  135  (•  2  (sqrt  (-  a  bl )  I 

(•  2  (sqrt  (•  a  b) ) ) I ) 
I 

) 

isatq  brq  (/  (arctan  (-  c  a)  (*  2  b))  2)) 

(satq  Sinb  (sin  (*  dtor  brqi ) I 
(satq  cosb  (cos  (*  dtor  brq) ) I 
(satq  sinb2  (*  sinb  sinbi) 

(satq  coab2  (*  coab  cosbl) 

(satq  sincos2b  (*  2  b  sinb  cosb)) 

(satq  sa;r  (*  2  (sqrt  (-  (*  a  stnb2)  sincos2b  (*  c  cosb2) ) I i ) 

(satq  BBnr  (•  2  (sqrt  (♦  (•  a  cotb2)  (•  -1  sincos2b)  (*  c  sinb2i))ii 
(raturn-froB  cov-to-allipsa  (list  brq  aiijr  sanr) ) 
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lisp  matrix  manipulation  routines 
</28/89 — s.c.  lent 

these  routines  operate  on  three (3)  ’stanclard  type'  lisp  objects  which  are 
subsets  ot  lisp  object  types: 

1.  scalars,  i.e.,  lisp  type  'nwiber' 

2.  vectors,  i.e.,  lisp  type  'simple-vector'  with 

length  >  1 

3.  matrices,  i.e.,  lisp  type  'array'  with 

rank  -  2  and 
each  dimension  >  1 

all  routines  accept  input,  and  produce  output  of  these  types 

unacceptable  inputs  produce  'NIL'  as  output,  there  are  cwo(2)  cases: 

1.  'non-standard'  Inputs 

2.  'standard'  but  mismatched  Inputs,  e.g., 
the  inverse  of  matrix (m  x  n),  where  m  o  n. 


routines  consist  of: 

1.  mx-mult 

2.  fflx-add 

3 .  sc-mx-«ult 

4.  mx-trans 

5.  mx-ldent 

6.  mx-inverse 


these  are  built  using  the  following  'sub-routines': 

1.  matrixp 

2.  array-row-el 

3.  arrsy-col-el 

4.  arrsy-row-dim 

3.  array-col-din 

8.  can-mx-tsult 
3.  can-mx-add 

8.  sgr-mx 

9.  mx-f actor 

10.  mx-subst 


(defv-.  matrlx-multlply  (a  bl 
(if  (not  (can-mx-mult  a  b) ) 

(return-froB  matrlx-multlply  NIL) 


(let  )(c)  (ij-llsti) 

(setq  c  (make-array  (remove  1  (list  (array-row-dim  a) (array-coi-dlm  bl ) ) 

) 
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2 


) 

(do  ((ind«xl  0  indexl  1))) 

( (>  indexl  (-  (array-col'din  b)  DM 

(if  (>  (array-coi-dim  b)  1)  (aetq  (list  indexl)) 

(setq  ij-list  <)) 


(defur.  raatrix*scaiar<-aultiply  (sc  mx) 

(if  (not  (and  (nmberp  sc)  (matrlxp  mx) ) ) 

(return-front  matrlx-scalar-sultlply  NIL) 

) 

(let  ( (1 ^-llst) (mx-n«w  (make-array  (array-<liaensions  mx) ) ) ) 

(do  ((indexl  0  ('^  indexl  1))) 

( (>  indexl  (-  (array-col -dim  mx)  1))) 

(if  {>  (array-col-dlffi  mx)  1)  (setq  ij-list  (list  indexl)) 

(setq  Ij-llst  0) 

) 

(do  ((index2  0  («  lndex2  1))) 

( (>  lndex2  (-  (array-diaenslon  mx  0)  1))) 

(if  (>  (array-dimension  mx  0)  1)  (setq  Ij-list  (cons  index2  i^-list)) 
) 

(setf  (apply  I'aref  mx-new  Ij-list)  (*  (apply  •*aref  mx  i^-list)  sen 
(setq  tj-list  (edr  l^-list)) 

) 

) 

mx-new 

) 

1 
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a 


;defun  matrix-transpose  (mx) 

(if  (not  (matrixp  mx) ) 

(retx2xn-2raai  matrix-transpose  NIL) 

) 

(let  (dj-list)  <mx-new  (aake-*array  (reverse  (array-<liAension$  mx) ) ) ) ) 

(do  (dndexl  0  (-*-  indexl  1))) 

{ (>  Indexl  (-  (array-col-dun  rax)  1))) 

(if  (>  (array-col -dim  rax)  1)  (setq  ij-list  (list  indexl)) 

(setq  1 j-list  () ) 

) 

(do  ((index2  0  lndex2  1))) 

((>  index2  (-  (array-dimension  mx  0)  1))) 

(if  (>  (array-dimension  ax  0)  1)  (setq  1^-list  (cons  lndex2  i^-list)) 

) 

(setf  (apply  #'aref  mx-new  (reverse  i}-list})  (apply  I'aref  mx  13-list)) 
(setq  13-list  (cdr  i)-list)) 

) 

) 

fflx-new 

) 

) 


(defun  raatrix-add  (mxl  ax2) 

(if  (not  (can-ax-add  nxl  ax2)) 
(return-fron  matrix-add  KID 


) 

(let 

(do 


) 


(  d j-list) (mx-new  (make-array  (array-dimensions  mxl) )) ) 
(dndexl  0  («■  indexl  1))) 

( (>  indexl  <-  (array-eol-dlm  mxl)  1))) 

(if  (>  (array-col-dim  mxl)  1)  (setq  1 j-list  (list  indexl)) 

(setq  ij-list  <)} 


(do  ((index2  0  (*  indBx2  1))) 

((>  indsx2  (-  (arrey-diaension  mxl  0)  1)}) 

(if  (>  (array-dimension  mxl  0)  1)  (setq  i j-list  (cons  index.?  ij-iist)) 
) 

(setf  (apply  #*aref  mx-new  ij-list)  («  <spply  #'aref  mxl  ij-list) 

(apply  S'aref  mx?  ij-list))) 


(setq  ij-list  (Cdr  ij-list)) 


) 


rax -new 

) 

) 


(defun  identity-matrix  (m) 

(if  (not  (and  (inteqerp  m) (>  m  1))) 

(return-front  identity-taatrlx  KID 

) 

(let  { (i  j-list)  (mx-new  (make-array  (listmv)))) 
(do  ((indexl  0  (-*’  indexl  l))) 
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((>  IrKiixl  (-  ■  1))) 

(setq  (list  Indaxl)) 

(do  ((index2  0  {**■  indcix2  1))) 

( (>  ind«x2  (*  a  1) )) 

(s«tq  ij-llst  (Com  lnd«x2  ij-list)) 

(Mtf  (Apply  #*ArAf  ox-rMw  ij-lltt)  (if  (•  Indaxl  lndex2)  1  0)) 
(s«tq  (Cdr  t^-^listn 

) 

) 

mx-n«w 

) 


(•l€f-an  matrlx-tnv€rt  <a) 

(if  (no^  (sqr*mx  4) ) 

(r«turn-fraai  m4trix*lnv«rt  KZL) 

> 

(let*  ((n  (ArxAydlMnslon  a  0)) 

(t«fnp  (mx'factor  a  nn 
(w  (car  taop)) 

(ipivot  (cadr  taatp>) 

) 

(if  (•  0  (caddr  taisp}) 

(rtturn-froB  aatrix-lnvart  NIL) 

) 

(mx*9u&st  w  n  ipivot) 

) 

) 


(dafun  matrixp  (a) 

(if  (arrayp  a) 

(cond  ( (slinplt*>v«ctor'‘p  a  > 

(if  <>  (arraydlMnslon  a  0)  0) 

T 

NIL 

) 

) 

( (aqaal (array-rank  a)  2) 

(if  (and  (>  (array-dlMnsion  a  0)  0) 
(>  (array-dlaansion  a  I)  0) 

) 

T 

NIL 

) 

) 

(T  NIL) 

) 

NIL 

) 

) 
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(dftfun  array-row*^!  (a  1) 

(apply  t'arcf  a  (if  (•  (arrayrank  a)  1) 
disc  (car  i) ) 
i 


(dafun  array'col'al  (a  1) 

(apply  I'araf  a  df  (•  (arrayrank  a)  1) 
(cdr  1) 

1 


(dafun  array^row'dlA  (a> 

(if  (•  (array-cank  a>  1) 

(arcay*dlaansion  a  0) 

) 

} 


(dafun  array *col -dim  (a) 

(if  (•  (arcayrank  a)  1) 

1 

(array^dliAanaion  a  (•  (arrayrank  a)  in 

) 

) 


(dafun  can-nx^^ilt  (a 

(if  (and  (macrixp  aMoMCrixp  bn 

(if  (•  (arraydimanaion  a  (*  (array*rank  a)  D) 
(arraydiAansion  b  0) 

) 

T 

KXL 


) 


NIL 


(dafun  can'-nx-add  (a  b) 

(if  (and  (sAtrikp  aMmacrixp  bn 

(if  (and  (•  (array-diBansion  a  0) 
(arraydiAansion  b  0) 


Mttrli  funetlofw 
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(•  (Array'^diMfuion  a  (-  (array-ranJt  a)  1)) 
iaxray-OiMnsion  b  (array-rank  b)  D) 

) 

) 

KIL 

» 

MIL 


ide/un  sqr-mx  (a) 

(If  (matrixp  a) 

(if  (•  (array^ranx  ai  2) 

(if  (•  (array^'dioanslon  a  0)  (array *diAanslon  a  D) 
T 

HZL 

) 

> 

NIL 


) 


J 


(dafun  mx-factor  (ax  ni 

(l«t  ( (awikodMcoliRaxuratio)  (roMmax)  (tasp)  (Istar)  (Iflag  1) 
(ipivot  (aaka<array  nn 
(d  (maka-array  nu 

(a  (aaka-array  (array^dinanaiona  nx) ) ) 

) 

(if  «•  n  Oi 

(raturn-fron  ax-factor  rUl) 

) 


(do  ( (1  0  (♦111) 

(rowmax  0  0) 

) 

( (•  1  nM 

(satf  (araf  iplvot  i)  1) 

(do  ((3  0  (♦  3  ini 
( (•  3  n) ) 

(satf  (araf  1  3) (araf  ax  1  j)) 

(aacq  roaaa  aax  roanax  (abs  (araf  w  i  j)))) 

) 

(Whan  (•  rowaan  0) 

(satq  iflaq  0) 

(Mtq  rowMx  1) 

) 

(aatf  (araf  d  1)  roMMx) 


(dc  ( (k  0  {*  k  1) )  I 
( (-  k  (-  n  1)  n 

(satq  colAax  (abs  (/  (araf  w  k  k) (araf  d  k) ) ) > 
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(Mtq  litar  k) 

(do  ( (  1  (♦  k  II  (♦11)11 
( (•  I  nil 

(sacq  awikod  (aba  (/  (araf  w  i  kl  (axaf  d  liin 
(«nan  (>  avikod  colaaxi 

(tacq  ooljujt  awikod) 

(sacq  iscar  1) 

1 

I 

(If  (•  eolaax  Ol 
(tacq  iflaq  Ol 
(proqn 

(If  (>  itcar  ki 
(proqn 

(tacq  Iflaq  (*  •!  iflaqi I 

(tacq  1  (araf  iptvot  Ittar) I 

(Mtf  (araf  Iplvot  litarl  (araf  tplvoc  ki  I 

(aatf  (araf  iplvoc  ki  ti 

(tacq  ctiv  (araf  d  tatarll 

(tacf  (araf  d  ittar)  (araf  d  k) i 

(tacf  (araf  d  ki  tai^l 

(do  ((;  0  (♦  3  1)1) 

( (-  3  nil 

(tacq  caaip  (araf  »  Ittar  3)) 

(tacf  (araf  w  ittar  3) (araf  »  k  3)) 

(tacf  (araf  w  k  31  taapi 

I 

I 

I 

(do  (d  (♦  k  II  (♦  1  1111 
( (•  1  nil 

(tacf  (araf  ■  1  ki  (/  (araf  «  i  kl  (araf  a  k  km 
(tacq  ratio  (araf  a  i  kii 
(do  ((  3  (♦  k  1) (♦  3  11)1 
((»  3  nl) 

(tacf  (araf  a  i  3)  (-  (araf  a  1  3)  (*  ratio  (araf  a  k 

31 1 1 1 

I 

I 

I 

I 

I 

(If  (•  (araf  a  (-  n  II (-  n  1))  0) 

(tacq  Iflaq  Oi 

I 

(lltc  a  iplvoc  iflaqi 


(dtfun  mx>aubtt  (a  n  iplvot) 

(lac  ( 

(b  (aaka'array  n  ; initial -alaatnc  Ol) 

(aln.  (aaka'array  (lltt  n  nil) 

dpi 

1 


(do  ((col  0  (♦  eal  III) 

( (•  col  nl I 

(tacf  (araf  b  coll  1) 

(tacq  ip  (araf  Iplvot  On 

(tatf  (araf  ainv  0  ool) (araf  b  ip) I 

(do  (d  1  (♦  1  II) 

(t«>  0  Ol 


( (-  1  nil 

(do  ((30  (♦  3  1)11 

(tacq  t»  (♦  ti*  (•  (araf  a  1  31  (•!•*  •l'"'  3  col) )  1 ) 


(tacq  Ip  (araf  iplvoc  D) 
itatf  (araf  ainv  1  col  i  ( 


(araf  b  ipi  am  11 


(tacf  (araf  ainv  (-  n  II  coll  (/  (araf  ainv  (-  n  1)  coll  (araf  a  ( 


(do  ( <  1  (-  n  2»  (-  i  > 

(  C  3) 

) 

( (•  1  -IJ ) 

Ido  ( (  ^  (♦  i  1>  (♦  )  ^ 

I  •  3  nn 

(s«tq  9\jm  (♦  t\M  (•  «inv  3  col)  i*r«f  w  i  ^)))i 

(*r*f  •inv  1  col)  {/  (•  (*r*f  *lnv  1  C0a.»  t\*Ri  (»r«f  w  v  . 


<s«tf  (4rtf  t  col)  0) 


•  inv 
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TRACK  Utility  Pr<jc«<lurt j : 


(dafun  9«tlAtlJ\q  (latl  Irvgl  dlst  brq  flag) 

.’flag  •  0  graat  clrcla  calc 
•  1  rhMnbllna 
brg  in  dagraas. 

(laf  I(lat2  latl)  (lng2  Ingl)  (rng  (ra*  (/  dlst  iO.lOTT)  360)  i 
(cbcg  (cosLltp  brg) )  ilatlt  latl)  (clatlt)  (ilatl) 

(srng  mil  (crng  ml)  (ding  ml)  (caa^  ml)  <clac2  mil 
I 

(If  (•  latl  90)  (sacq  latlt  19.9)) 

(if  (•  latl  -90)  (satq  latlt  -89.9)) 
isatq  clatl  (cotuap  latlt)) 

(if  (•  flag  Ol  ;  gnat  clrcla  calculation 

(progn  (if  (>  rng  180) 

(progn  (aatq  rng  (-  360  rng)) 

(aatq  brg  (raa  (*  180  brg)  360) ) 

(aatq  ebrg  (coaLlip  brq) i 
I 

) 

(If  (•  clatl  0) 

(progn  (if  (•  (tlnLlap  brg)  1) 

(aatq  lat2  (-  latl  rng) ) 

(aatq  lat2  (->  latl  rng) ) 

I 

(aatq  lng2  Ingl) 

I 

(progn  (aatq  alatl  (ainLiap  latlt) ) 

(Mtq  arng  (ainLiap  rng)) 

(aatq  crng  (ceaLiap  rng)) 

(aatq  lat2  (-  90 

(aeoaLlap  (fnAba  (*  (*  clad  arng  cbrg) 

(*  alatl  crng) 1 1 1 1 1 

(aatq  elat2  (eoaUap  lat2)) 

(aatq  ding  (aeoaLlap  (fnAba  (/  (-  crng  (*  alatl 

(ainLiap  lat2ui 
(•  clatl  elat2i ill) 

(if  «  (alnUap  birg)  0) 

(aatq  lng2  (fnCor  (-  Ingl  ding))) 

(aatq  lng2  (fnCor  i*  Ingl  dingo) 

) 

I 

)  :lf  elatl«0 

I  ;  progn  mg  >  180 

)  ;lf  flag  •  gc 

(If  (•  flag  1)  .■rtaabliiM  calc 
(progn  (if  (or  (•  brg  90) 

(-  brg  270) ) 

(progn  (aatq  lat2  latlt) 

(If  (•  brg  270) 

(aatq  Ingl  (fnCor  (-  Ingl 

(/  rng  clatl) ) ) ) 

(aatq  Ingl  (fnCor  (*  Ingl 


(/  mg  clatl)))) 

) 

)  ;and  than  eonatmct  br^90  or  brg«270 
(progn  (aatq  latl  (■»  latlt  (*  mg  cbrg) ) ) 

(If  (>•  (aba  latl)  90) 

(progn  (If  (>  latl  0) 

(aatq  latl  90) 

(aatq  latl  -90) 

) 

(aatq  Ingl  0) 

) 

(progn  (aatq  tiap  (log  (/  (tanLlap  (-  41  c  0.5  latltiii 

(tanLlap  (-  4J  (•  o.S  latliiiiii 
(aatq  Ingl  (fnCor  (*  Ingl  (*  CLB.radToCag  tamp 

(tanLlap  Prgi i i i i 

) 

I 

I  .and  alaa  conatruct  brg>90  or  brg>270 

I 

I 

I  .  If  flag-l 
(lift  latl  Ingl I 
I  ;  lat 
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.:ie:ur.  gaciUigBr?  liacl  Ingl  lac2  lnq2  flag) 

:  flag  •  0  graac  circla 
•  1  rhxaablina 

(lac*  ((rng  0)  (brg  0)  (dlat  (aba(-  latl  lat2))l 

(ding  (aba(fnCor(-  Ingl  lng2l)i)  (a)  (y)  (lacit)  (clac2  (coaLlap  lac2  i) 

) 

(It  (>•  (*  dlat  ding)  0.0001) 

(progn  (it  (•  flag  0) 

(pcogn  (If  (•  ding  0) 

(progn  (if  «  latl  lat2) 

(tatq  Brg  0) 

(latq  brg  ItO) 

) 

(satq  mg  dlat) 


(coiLltp  ding) ) ) ) ) ) 


(progn  (If  (•  (aba  latl)  90) 

(aatq  latlt  c  99.9 

(/  latl  (abs  latl I ) ) ) 

(aatq  latlt  latl) 

) 

(aatq  clad  (coaLlap  latlt) ) 

(aatq  alacl  (alivLlap  latl)) 

(aatq  alac2  (ainLlap  lat2) ) 

(aatq  rng  (acoallap  (fnAba(*  (*  alacl  alat2) 

(*  clad  clat2 

(aatq  brg  (acoaldap  (fnAba(/  (-  alat2 

(*  alttl  (coaliap 


mg) ) ) 
mg) ) ) ) ) ) 

datalina  eroaalng 
;  la  tnif  progn  nacasaaiy 


(*  clad  (alnblap 

(If  (or  (>»  liig2  -»0l 

(<•  lagl  90))  ;otbar  than  aaat  or  xaat 

(pcoffi  (If  «  lng2  logl) 

(aatq  brg  (-  3(0  brg) ) 

(progn  (if  (and  (>  lng2  90) 

«  Ingl  -90)) 

(aatq  brg  (-  3(0  brg) ) ) 


)) 


) ) 


)) 


) )  ;  If  flag-0 
(If  (-  flag  1) 

(progn  (If  (or  (•  (aba  lad)  90) 

(•  (aba  lat2)  90))  .North  or  South  pola 
(progn  (tatq  mg  dlat) 

(If  (>  lst2  latl) 

(aatq  brg  0) 

(aatq  brg  ISO) I 

(progn  (aatq  a  (log  (/  (tanuap  (-  «9  (•  O.S  lad))) 

n  (-  as  (•  0.5  lat2)))))) 

(aatq  a  (*  a  (aj.radToOag) ) 

(aatq  y  (fnCor  (-  lng2  Ingl))) 

(aatq  brg  (fnhretan  a  y) ) 

(If  (or  (-  brg  90) 

(-  brg  2'>0) ) 

(aatq  mg  (•  clat2  ding) ) 

(tatq  mg  (aba  (/  dlat  (coaLlap  brg) ) ) ) 


) ) 


) ) 


(aatq  rng  (•  (O.IO")")  rng)) 
(litt  mg  brg) 

)  .  lat 
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DaMction  Ncpenar 


Wad.  mu  21. 1M0 


(Sdtq  ddtdctionRaportdr  (kindof  qdndrdlPldtform) I 


(cScfobfun  (tixist  ditdction«aport«rl  (iiut-llic) 
lusudl-dxist  init-lUtl 


(hdv#  'rtport-typd  'POS^VtL  I  pos-pox  only.  ros*vtt-po5  ■>  vei 

(hdv*  'pojition«l-unc  10  I  ;  no 

thavc  '>pd«d-vinc  3  I  ktx 

(nava  'cour»a-unc  30  )  •'  dagiaas 

(hava  'axpMpDalay  il 
(Mva  'fW  0.41 

(Sava  ‘atanTraeXp  1) 

(sa*.q  dataNantlitt  (appand  OataNaMLlst 
• ( : raport  a 

(raport-type  positional -unc  spaed-unc  couria-in: 
axpRapOalay  PAR  itartTracxp) ) i i 
(satq  Oatavalualiit  (appand  dataValoaiiat 
( :rapdrts 

{.  raport -typa  ,posttlonal-unc  ,spaad-unc  .course-ur.c 
.axpRapSalay  .FAR  , startTrackpi 1 1 1 
(satq  dataTypalUt  (appand  dataTVpallst 

'  craports  (data  data  data  data  data  data  datai  1 1 ) 

(satq  dataOsxtlist  (appand  dataTaxtList 

•((raports  CRIPCRT  TYPE-  ’POSITrON  EAJOR"  *SPEE5  ERROR" 
•COURSE  ERROR*  "Exp  Procassing  Oaiay" 

"Falsa  AlaiP.  Rata*  "Tracx  Imtiaticf;  1 1  > 
(satq  dataTaisplataTaxt  (appand  dataTaa^lataTaxt 
' ( : raports 

(CPOS  or  POS-VEL"  "(PCS  ,  POS"VEi:*) 

CNactieal  Kilas*  "(0  -  1000:*| 

(•Knots-  *10  -  3:i-i 

("Oaqraas*  *10  -  180!*) 

CHoars"  *10  -  100]*) 

("FA  par  oay*  *10  -  unllaatad: *) 

("Singla  Conta)rt  to  IracA*  *0  -  no,  1  - 


POS-pos  only.  POSiVEL-pos  •  val 

nn 

kts 

dagxaas 


yas*> I ) ) I 

Cat*  ( (rt-dasiqnators  ' (raports-rb  n 

(ro-valuas  (list  d-.sdr.yRSdata) ) 

(rs-nanas  '  (dmryRBdats) ) 


(updata-rb-lists  rb-dasignators  rb*valuas  rb-namas) 


(defotJjn  (satForStart  datattionRaportar)  () 
;  isia. -set For Start  j 
(Save  '  tort  itadTAP  FA?) 
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2 


(dafobfun  (gatlUportOalay  datactlonAaporwir)  Uoptlonal  targatTypa) 
(raturn-fron  gatRaportOalay 

(axponantiaiOraw  axpRapOalay) 

) 

) 


(dafobfun 

Cat* 


(maka*obu*raport  datactioriAaportar) 


( (csa 
(sp2 
(lat 
(ing 

(hr: 

(tarTatNama 

(cat 


(platfonStataParms^caa 
(platfermStataPanfs*spd 
(platfoitRStataPana^lat 
(platfornStataPanBa-lng 
<plat  f  orB^tataPaxvifhgt 


(tUM  targat-pama  aoptional  KF2rp> 
tar^at^parms) ) 
tar9at-pafr!S) ) 
tarqat-pana) ) 
targat-paims) ) 
targat-paraisj ) 


(platfcrxStataPams-platfonMM  targat-pama) ) 
(maJca-obu-catagory  :pirmadp  nil 
:locXadp  nil 
: cluatar-count  0 
laatoc-^count  0)) 


(sansor-fiald  (tBaXa-obu-aansor 
:naina  (princ*to*string  namai 

: t;ngla'-raport-to-trackp  (not  (agualp  ttartTrackp  0)))> 
spatia:  •<iata*fi.ald 


) 


2iZ) 
) ) 


(if  (agualp  raport*typa  *P0S*VCI) 

(satq  caa  (aod  («  eta  (•  (randan  (*  2.0  coursa-^unc) )  ccurta^unc) ) 
tpd  (max  0  (*  spd  (•  (random  (*  2.0  tpaad*unc))  tpaad^uno) 


(satq  caa  nil 
spd  nili 

) 

(multipia'valua-satq  (lat  Ing) (contactPctitionDraw  lat  Ing 
positional -unci l 

(satq  fpatial-data-fiald  (maka-obu-data-fiald 


:typ« 

raport-typa 

:obs-tla* 

tima 

:lac 

lat 

anfl 

Ing 

.'bi^ 

0 

:ma) 

positional  •*unc 

:ain 

potitional-unc 

:eM 

csa 

: eaa-unc 

count- unc 

:spd 

tpd 

:tpd-unc 

spaad-unc 

( raturn- f roB  maka-obu-raport  (maka-obu-contact 

;id  nkl 
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Sataaion  M«Mn*r 


Wad.  Mv  11.  itM 


1 


) 


:  r«e«lpt-tlM 
;  track-udociatlon 
; aanaor 

: cataqorlxatlon 
;  tpacial-data 
.-alcituda 


tlaa 

tar^atNaaw 

aanaoi-tlald 

cat 

apat ial -data- { i e 1 d 
h9t 

mrp 


Idafofcfun  (aaka-(alsa-obu-r*port  dttactlonPaportcr) (lat  Ing  uncertainty  ctlna 
targatTypai 

(lat  ( (lalaaTargatStata  (qatralaatar^atstata  lat  Ing  uncertainty  taigctType i i ) 
(aaka-ofi. -report  ctUM  {a.saTargatStatei 
I 

I 


Idarccfu.-:  (gatra. aatargatState  detactlonRaportat)  (lat  Ing  uncertainty  targatTypa) 
(■ultiple-valua  cind 
(lat  Ingi 

(cor.taetPoiilionCrax  lat  Ing  uncertainty) 
caturr-lroB  getFal aaTargatStata 
(make-plat  formStataParxis 
:lat  lat 
:lng  Ing 
:ngt  C 

;cae  '♦  (ra.-.dor  l.Oi  363) 

:spd  10 

:platfcnriNaina  'FA 
(targatT-jpa  targe. Type 
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SiruellirM 


Tliu.Mnrl*L  1M0 


1 


(defstruct  coast-avcit 
tloe 
object 
procedur* 
data 

updateList 


(defstruct  coaat-nassaqe 
land-tiM 
racalpt-tlaa 
typa 

aandar 

eontant 

siia 

tranaalsslon-path 
tzansBi salon-count 

) 


Tlaa  messaga  sant. 

TUu  nassa^a  racalvad  at  last  node. 
E.g.  *Oatact-asg 


:  MBs 


(defstruct  obu-contact 
Id 

racalpt-tlsM 

tzack-assoctatlon 

sansor 

cata^orlzatlon 
spatial -data 
altltuda 
BTOrp 

I 

(dafatruct  obu-tzack 
Id 

nunbaz-of-contaets 
haad-stata-covazlanca 
haad-contact-ld 
haad-spatlal -data 
tall-stata-covazlanca 
tall-contaet-ld 
tal 1 -spat lal -data 
altltuda 

(contacts  nil  :typa  list) 

) 

(dsfstzuet  obu-data-flald 
typa 
obs-tlaa 
lat 
Ing 
bzq 

■aj 

■In 

esa 

caa-une 

spd 

spd-une 

I 

Idafstzuct  obu-stata-flald 
zaf-lat 
zal-lng 

(stata  nil  :typa  (azzay  float  4  )) 

(covazlanca  nil  ;typa  (azzay  float  '(4  4)1) 


(dafstzuct  obu-sansoz 

nans 

slndla-zapozt-to-tzaclsp 

) 

(dafstzuct  obu-catadozy 
plnnadp 
lockadp 
assoc-count 
clustaz-count 
aobldulty-llst 
anbldulty-zasolutlon 
anblgulty-zasolutlon-tlsM 

) 
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ProMUNy  luneUoiw 


Prt,M«r23, 1N0 


S 


(ctefun  contactPositlonDraw  (lat  Inq  rng) 

;  a  draw  from  a  circular  normal  --  rnq  is  Interpreted  as  a  2  sigma  confidence 
;  interval 

(let  ( (randomRange  (min  (•  (/  rng  2)  (sqrt  (•  -2  (log  (random  10))))) 

(•  4  mg))) 

(randomAngle  (randoaCsa)) 

) 

(return-from  contactPositlonDraw 
(values-list 

(getlatlag  lat  Ing  randomRange  randonAngle 

•greatClrcle*) ) ) 

) 


SPA  ruMmoMS 


(defun  expandAOUpos  (vel  tau  timelate) 

(lat  ((ratio  (/  tiaalate  tau)) 

(factor  (*  2  pi  vel  vel  tau  tau))) 

(•  factor  (»  ratio  -1  (exp  (•  -1  ratio) ) ) ) 

) 


) 


(defun  expandAOUpos Vel  (vel  tau  timaLate) 

(let*  ((ratio  (/  tlMl.ate  tau)) 

(factorl  (*  2  pi  vel  vel  tau  tau) I 
(factor2  (•  2  (exp  (•  -1  ratio)))) 

(factorl  (*  C.S  (exp  (*  -2  ratio)))) 

) 

(♦  factorl  (♦  ratio  factor!  -1.5  (•  -1  factor!))) 

] 

) 


(defun  getRandomPointlnRagion  (region) 

(let*  ( (regionParms  (getRagionParms  ragi(}n)) 

(minlat  (region-paims-minLat  regionParms) ) 

(maxlat  (region-paims-maxLet  regionParms) ) 

(sUnLng  (region-parms-minLng  regionParms)) 

(saxLng  (re^on-parms -maxing  regionParms)) 

(ckStuff  (rogion-paxms-ehec)^s  regionParms) ) 

(thePoint  nil) 

I 

(if  (>  sUnlng  maxing)  (setg  mining  (-  mining  360))) 

(do  ( (insidep  nil) ) 

(insidap) 

(setq  thePoint  (list 

(-  minlat  (*  (-  maxlat  mlnlat)  (random  1.01)1 


(*  mining  (*  (-  maxing  mining)  (random  1.0))))) 
(setq  insidap  (chec)ilflnside  thePoint  c)(5tuff ) ) 

) 

(return-from  getRandomPointlnRcgion  t)\aPoint) 

) 

) 


90 


APPENDIX  C;  ITERATIONS  TO  ACHIEVE  STEADY  STATE  GAIN  VALUES 


The  model  in  this  appendix  calculates  the  percent  differece  between  succesive  iterated  values  of  Kalman  gain  Ka. 
These  values  are  tabulated  for  various  ranges  of  interarrival  time  and  sensor  AOU  size. 


n  0..25 

A  =  50 

u 

100  a  .25 

»  -  e3  =  / 

2  a 

rl  =  25-u^ 

vIq  =  rl  v2o 

^  0  v3o  ■  r3 

42 

=  -  •  1  -  e«j>i  O' A  j  j 
a 

43  =  exp[  -O’ A 

2 

2 

2 

ql  ^  •  A  -■ 

2  a 

a 

2-  1  -  expi  O"  A 

'  ‘ 

1  -  exp'  Of2-A  : 

1  ]-  — .5  -  expi  -a-Aj  4  .5-exp' 
a 

a-2-A;ii,3  = 

a  ‘ 

1  -  exp:  -a- 2- A 

n  -H 
n  -H 


v3 


n  -H 


vl„  42-»2-v2„  -hil  j':  j  ^3^ •v3„ -ht3  vl„  »2- v2,  -if  »2 ]“•  1^3  ['v2„  -t^2-v3„ ■hl2 

^  ''*0  -(f  -h)l  -HI  -Hl3  -iTa  j-i  ^  v2^  -(^2-v3^  j-^3  -hi2  f 

v2„  .t^2-v3^’-*3  ■ts2l-fl-, - ^ -  ^ - 7l 

L''*o'^‘^2-v2^-tf«2’  •v3^-hll  -tTlj-|^i^3]  •v3^-ttl3^t3;-ffv2^'t^2'v3„'!-«3-hi2 


-  i  v2||  -f^2- v3||  ^3  •♦<|2  4  vlji  +2’^2‘ v2||  4  42|^‘v3|j  -hjl  |•1  [  43]^*  v3||  -^3  1  -trl  •]  J  43  v3|j  ^ij3  I  i 


i  vl^ +2-42-v2j,  4  42^ -vSij -hjl  -JtI  '•  [  43]^ -ttjS  -Its  I  ■[[  *2^  442'v3^j’43  -hj2  1^ 


vin  kl  -kl  , 

k'„  ^  TT  '  ^  >  ••*  «»«,  =  ■  ^-  -•><» 

Sensor  AOU  =  1  nm 


A=l-lO'^  • 


A  =0  01  •  A  =0.05 


A  =0.1  .  A  =0.5 


A  =1  • 


Din;  • 


S:-I-I.ViW. 

g3-X-JAl.». 

gai-i-iit. 

XSytUL 

aui2U< 

wtiiM, 

latfa*. 

anxwsr 

KHLSoa 

wtmoi 

■JgWlM 

Bnn»3:K-l 

»ME»H 

Ditt, 


h  98  037619 

mi':?!':-: 

UftUlM. 

WffKtfl 

Bjmj 

■•*>3.1.10.8 

BaUUI 

■■nam 

■imn-M 

WKKKa 

UUtiiU 

iM-rkM 

■«iEa»j 

nn; 


g-rag-?:-: 

BSIiUUlJ 

gaaiM 

BBUU2J 

ataaij 

BMPiJtl 

KSL^mi 

ttffK-avj 

OMUiU 

BSiima 

■man 

HiSaUU 

Difl;  I 

-  33.154*22 

-T'5!ii<y3 

-7.0ll60i 

TSWITg 

-I  284654 

-6  2^8693 

-  a.0j562l 

-O.oWioi 

'TnVSTJT 


Din; 

0 

Din; 

,aEZSOHi 

UtaUBBI 

TT"in— 

-5.581487- 10  * 

-2.082199-10'’ 
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APPENDIX  D:  KALJVIAN  GAIN  VERSUS  INTERARRIVAL  TIME 


This  appendix  shows  the  relationship  between  the  Kalman  gain  and  the  interarrival  time.  This 
information  can  be  used  to  select  a  range  of  time  values  to  tabulate  preprocessed  variance  data. 


n  =  o..a 


a  =  10 


u  :  50 


a  =  25 


vIq  ;  rl  v2q  =  0  v3q  =  r3 


42  =  i-' I -expi'-o'A’I  43  =exj>[-a’4i 
a  ' 


rl  =  .25'u^  ^  2-0 


q3  =  .5-  —  •[  1  -  expf -a'2- A” 
a 


ql  2'[  1  -  e*p[ -a’A]]  -  .5'[  I  -  «xpi -a‘2*Aj  jji  ll 

■  Q  "  '  ” ' ! I 


q2  ■  “2'-  ■*  '  'O' a]  -tf  -S-eipf  -  0‘2' A  jl  j 

a 


a  -H 
a  +1 


i  -H 


I  :  I  Vl„  +2'42'v2„  42i^'v3„  -tql  |•[[43f•v3,-^t|3  ■ 

i(*l„+2-42-v2,-l{42^v3„  -tql 

•r3 

-[[v2,-l42-v3„]-43lq2’^ 

I  [vl^4-2-42'v2„Hf42f'v3„-(sHTl 

!  ^'’w7  4^7. lA*?  4n^  ]*rt  * 

t3 

-[[v2,-I42-v3 

b]*43  ^q2f 

1  -  "  '"^*^"^J'‘[w„«.42-.2.^42f 

**3j  -tql  -ItI  !*[[  43f 'vSj  -Iq3  -tt3 

R 

»2„-t42'v3,i.43-tq2]^ 

vV„  ■«-42'V2„  i  *2f-y3„  -tqt  ->rt  jj'  43f -H,}  *3  j  -[[  v2„  ■442'v3„]-43  -*q2  ] 


kl„  =  -T? 
“  rl 


v2 

k2a  =  — 
“  r3 


k2b. 


KDATA  -  READniN(Kate«n)  _ <> 

titm  -  KDATA  Kl  =  KDATA^ 

Sensor  AOU  =lnm 


v2_  v3„ 

0  TT  “n  '1T  APPENDf*N(KaJiiiM,)  =  [A  u  kljj  kZa^j  k2h2j  Ojj  j, 

o>l 


n  -  0  ..luq^KDATA 
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9' 


APPENDK  E;  OFFSET  ERROR  OF  FILTER  POSITION  DISTRIBUTION 


This  appendix  provides  indications  of  how  far  from  the  target’s  true  position  the 
filtered  position  is  displaced  by  the  lOU  process  adjustment  of  velocity  to  account  for 
the  possibility  of  a  manuever.  This  assumes  the  target  is  actually  moving  with 
constant  course  and  speed.  The  first  graph  cover’s  the  entire  range  from  Inm  to 
lOOOnm  while  succeding  graphs  show  only  the  neighborhood  of  the  maximum  value 


Offset  Error  for  10  kt  Target. 

interarrival  Time:  5  hours. 
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Offset  Error  for  30  kt  Target. 

Interarrival  Time;  5  hours. 


Offset  Error  for  3  kt  Target. 

Irrterarrival  Time;  1  hour. 
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to  Filtered  AOU. 


Offset  Error  for  10  kt  Target. 

Interarrival  Time:  1  hour. 


10  10.5  11  11.5  12  12.5  13  13.5  14  14.5  15 

Sensor  AOU  2-aigma  radius  (in  nm). 


Offset  Error  for  30  kt  Target. 

Interarrival  Time:  1  hour. 


w  3.07 


11  11.5  12  12.5  13  13.5  14 

Sensor  AOU  2-8igma  radius  (in  nm). 


APPENDIX  F;  ERROR  BETWEEN  FILTERED  AND  ESTIMATED  SPA  SIZE 
This  appendix  shows  the  difference  in  standard  deviation  between  the  actual 
filtered  position  distribution  a^id  the  estimated  distribution.  The  data  tabulated  shows 
more  complete  data  for  the  cases  modeled.  This  includes  the  observed  AOU 
(Obs.AOU)  and  the  predicted  AOU  (PRED  AO);  the  mean  value  of  the  standard 
deviation  calculated  for  30  runs  of  48  hours  of  300  observations  for  the  position 
predicted  by  the  lOU  model,  the  contacts  position,  and  the  filtered  position;  as  well  as 
the  actual  mean  speed  of  the  contact  over  the  30  runs  and  the  mean  filtered  value. 


Time=4  Hours,  Target  Speed =10kts. 

Parammra  Mftm-  Zi.  VolOka. 
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Time*.1  Hour.  Target  Speed =lOkl9. 

Pmmnmun:  valO  Ua 


Data  for  on  lOU  process  motion  model  beta=  25  and  sigma  based  on  10  knot  average  speed 
Time  interval  between  observations  of  4  hours: 

Standard  Deviation  of  error  in  nautical  miles:  Speed  in  knots. 


Prediction  Contact 

Filter 

Contact 

Filtered 

mean 

mean 

mean 

SPA 

%Error 

Obs.  AOU 

Pred.  AO 

mean 

mean 

14.941 

11.957 

11.032 

11  499 

-4  06122 

25 

30.684 

2.959 

3.044 

20  041 

19.071 

16.716 

17  18 

-Z70081 

40 

34.216 

2.975 

3.145 

22.194 

23.148 

19.693 

20.594 

-4.37506 

50 

36  52 

3.078 

3.078 

33145 

43.599 

32.113 

35.113 

-8.54384 

100 

47.234 

302 

2.635 

52.749 

81.301 

53.04 

59.109 

-10.2675 

200 

67.285 

2.92 

2.118 

92.721 

160.022 

93.447 

103.544 

-9.75141 

400 

108.788 

2.992 

1.881 

149  547 

245.976 

151.585 

148.352 

2.179276 

600 

152.841 

3.061 

1.895 

21.938 

11.925 

11.197 

11  499 

-2.62632 

25 

30.684 

10.133 

7482 

27.577 

19.357 

17.561 

17.18 

2.217695 

40 

34.216 

10.026 

7.087 

27.27 

22.54 

19.703 

20.594 

-4.3265 

50 

36.52 

9.997 

6.81 

40.463 

43.917 

34.288 

35.113 

-2.34956 

100 

47.234 

9.952 

6.369 

57  402 

84  078 

54.921 

59.109 

-7.08522 

200 

67.285 

9.943 

5.999 

95.207 

154.341 

95.083 

103.544 

-8.17141 

400 

108.788 

10.307 

5.925 

146  205 

243.202 

147.258 

148.352 

-0.73744 

600 

1SZ841 

10.076 

5.958 

5641 

13.4 

13.682 

11.499 

18.98426 

25 

30.684 

29.933 

21.703 

59.391 

21.688 

22.186 

17.18 

29.13853 

40 

34.216 

29.931 

20.036 

63.26 

26.691 

26.537 

20.594 

28.85792 

50 

36.52 

29.962 

20.124 

78  533 

47.705 

47.533 

35.113 

35.37151 

100 

47.234 

30.019 

18.783 

101.683 

89  644 

78.28 

59.109 

32.4333 

200 

67.285 

30.001 

18.033 

133.696 

169.918 

121.254 

103.544 

17.10384 

400 

108.788 

29.908 

17.635 

169  943 

233  559 

160.386 

148.352 

8111788 

600 

152.841 

30.072 

17.556 

Time  interval  between  observations  of  2  hours: 

Standard  Deviation  of  error  in  nautical  miles: 

Speed  in  knots. 

Prediction  Contact 

Filter 

Contact 

Filtered 

mean 

mean 

mean 

SPA 

%Error 

Obs.  AOU 

Pred.  AO 

mean 

mean 

7  08 

4.859 

4.547 

4.658 

-2.383 

10 

14.219 

3.041 

3374 

10  401 

9.202 

8.001 

8.471 

-5.54834 

20 

17.145 

3.017 

3.416 

16.528 

20  859 

15.156 

17.217 

-11.9707 

50 

24.218 

3.023 

2.904 

25.218 

40.585 

24.689 

28.317 

-12.8121 

100 

33.625 

3.068 

2.421 

43  418 

78  444 

43.725 

47  058 

-7.08275 

200 

50.636 

3.065 

2.144 

58.566 

115.538 

58.707 

64.879 

-9.51309 

300 

67.655 

2.897 

1.979 

9.704 

4.805 

4  583 

4  658 

-1.61013 

10 

14.219 

10.066 

8.383 

12.211 

8.83 

7806 

8471 

-7.85031 

20 

17.145 

9.972 

7.805 

19  499 

21.68 

16.535 

17.217 

-3.9612 

50 

24.218 

9.964 

7004 

28.038 

39  535 

25.836 

28.317 

-8.76152 

100 

33.625 

10.025 

6.615 

43  761 

78.011 

42.761 

47.058 

-9.13128 

200 

50.636 

9.958 

6.384 

64.397 

115.971 

63.61 

64.879 

-1.95595 

300 

67.655 

9.975 

6.213 
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22.67 

5.196 

5 

25  584 

10.066 

9. 

32.774 

22.442 

20. 

47  178 

43  851 

35. 

64.995 

83.895 

56. 

77.334 

119.128 

71 

242 

4658 

12.53757 

858 

8471 

16.37351 

253 

17.217 

17  63373 

221 

28.317 

24  38111 

581 

47.058 

20.23673 

016 

64.879 

9  459147 

Time  interval  between  observations  of  1  hours: 
Standard  Deviation  of  error  in  nautical  miles: 


Prediction 

Contact 

Filter 

mean 

mean 

mean 

SPA 

%Error 

3  607 

2,323 

2.146 

2296 

-6  5331 

5.215 

4.445 

3.803 

4.159 

-8.55975 

8.437 

10  543 

7,439 

8  435 

-11.8079 

11.984 

19  878 

11.43 

13.716 

-16.6667 

18.691 

37  993 

18.477 

22.018 

-16.0823 

25.46 

58.34 

25,401 

29.352 

-13.4608 

4.568 

2.393 

2.243 

2.296 

-2.30836 

6.018 

4.461 

3.846 

4.159 

-7.52585 

9  107 

10.411 

7.472 

8.435 

-11.4167 

13.545 

19.994 

12.392 

13.716 

-9.65296 

19  93 

37.793 

19.213 

22.018 

-12.7396 

25.759 

54.988 

25.225 

29.352 

-14.0604 

934 

2.448 

2.452 

2.296 

6.794425 

10.781 

4784 

4.551 

4.159 

9.425343 

14.764 

10.961 

9.315 

8.435 

10.43272 

20  459 

21.364 

15.801 

13.716 

15.20122 

28395 

39.358 

24.728 

22.018 

12.30811 

36  583 

57  941 

33,572 

29.352 

14.37721 

Time  interval  between  observations  of  .5  hours: 
Standard  Deviation  of  error  in  nautical  miles. 
Prediction  Contact  Filter 


mean 
1.03 
2.563 
3  622 
5.894 
8.469 
11.295 


mean 
0  488 
2.203 
4.095 
9.608 
18,734 
27  866 


mean 

0.474 

1.847 

3.014 

5.522 

8.256 

11.156 


SPA 

0.485 

2.033 

3.05 

6.688 

10.542 

13.745 


%ErTOr 

-2.26804 

-9.14904 

-1.18033 

-17.4342 

-21.6847 

-18.8359 


1.533  0  487 

2.839  2.205 

4  4.182 

6.342  9  788 


0.475 

0.485 

-2.06186 

1.874 

2.033 

-7.82095 

3.131 

3.5 

-10.5429 

5,743 

6  688 

-14.1298 

10 

14.219 

30.061 

24285 

20 

17  145 

29.933 

21,955 

50 

24.218 

30.047 

20.111 

100 

33  625 

29.957 

19  565 

200 

50.636 

29,938 

18.814 

300 

67.655 

29.979 

18  895 

Obs  AOU 

Pred.  AO 

5 

6  429 

10 

8.15 

25 

12 

50 

16.684 

100 

24.227 

ISO 

31.113 

5 

6429 

10 

8.15 

25 

12 

50 

16.684 

100 

24.227 

150 

31.113 

5 

6.429 

10 

8.15 

25 

12 

50 

16.684 

100 

24.227 

150 

31.113 

Obs.  AOU  Pred.  AO 
1  2.162 

5  3  755 

10  5  499 

25  8.187 

50  11.759 

75  14768 

1  2.162 

5  3.755 

10  5.199 

25  8.187 


Speed  in 

knots. 

Contact 

Filtered 

mean 

mean 

3.079 

3.489 

2.989 

3515 

2.96 

3.119 

3.032 

2.737 

2.984 

2.329 

2.991 

2.215 

9.988 

8.922 

9.904 

8.387 

10002 

7.7 

10.005 

7.255 

10.091 

7,009 

9.946 

6.891 

30,02 

26.103 

30.055 

24.381 

29  998 

22.467 

29.948 

21.356 

29  953 

20.68 

30  002 

20.393 

Speed  in  knots. 

Contact 

Filtered 

mean 

mean 

3.046 

3.178 

2.988 

3.437 

3 

3.298 

3.033 

2.875 

3.028 

2.58 

3.01 

2.435 

10.011 

9607 

9.984 

8  905 

9.944 

8.461 

10.024 

7.945 
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9101  18558  8.7  10542  -17473 

11.972  27  983  11.653  13.745  -15.2201 

051  0.513  0.485  5.773196 

2.365  2.207  2.033  8.55878 

4  456  3.791  3.5  8.314286 

10  247  7.273  6.688  8.74701 

19.553  11.421  10.542  8.338076 

28  111  14.945  13.745  8  730447 

Time  interval  between  observations  of  .1  hours: 
Standard  Deviation  of  error  in  nautical  miles: 

Prediction  Contact  Filter 
mean  mean  mean  SPA  %Error 
0.45  0.43  0  34  0.377  -9.81432 

1.155  1.921  1.059  1.263  -16.152 

1.689  3  728  1.621  1.995  -18.7469 

2.084  5.502  2.034  2.573  -20.9483 


0495 

0429 

0.348 

0.377 

-769231 

1.241 

1.934 

1  112 

1.263 

-11.9557 

1.761 

3  748 

1.665 

1.995 

-16.5414 

2.221 

5.549 

2.144 

2.573 

-16.6731 

0  778 

0  466 

0428 

0.377 

13.52785 

1.721 

2.005 

1.368 

1.263 

8.313539 

2408 

3.832 

2.104 

1.995 

5463659 

2.947 

5.618 

2.685 

2.573 

4.352895 

Time  interval  between  observations  of  .05  hours: 
Standard  Deviation  of  error  in  nautical  miles: 

Prediction  Contact  Filter 

mean 

mean 

mean 

SPA 

%Error 

0322 

04 

0.274 

0.312 

-1Z1795 

83 

1  843 

0.795 

0.978 

-18.7117 

0.359 

0.41 

0.291 

0.312 

-6  73077 

0.871 

1  855 

0.82 

0.978 

-16.1554 

0  534 

0  442 

0.37 

0  312 

18.58974 

1  176 

1.908 

1.045 

0.978 

6.850716 

3  513 
4.761 
6.158 
9329 
13.198 
16  49 


50 

1 1  759 

10.05 

7625 

75 

14  768 

9.981 

7512 

1 

2162 

30  01 

28.592 

5 

3.755 

30.038 

26.093 

10 

5.199 

30.015 

24  905 

25 

8.187 

29.992 

23.28 

50 

11.759 

29  652 

22.652 

75 

14.768 

30.026 

22.335 

Obs.  AOU 

Pred.  AO 

Speed  in  knots. 
Contact  Filtered 
mean  mean 

1 

0.592 

3.002 

3.192 

5 

1.504 

3.021 

3  081 

10 

2.22 

3.003 

2.847 

15 

2.76 

2.995 

2.734 

1 

0.592 

10.003 

9568 

5 

1.504 

10.005 

0.900 

10 

2.22 

10.026 

8.711 

15 

2.78 

10.015 

8563 

1 

0.592 

29.986 

28414 

5 

1.504 

29  998 

26  532 

10 

2.22 

30  037 

25.88 

15 

2.78 

30  008 

25  47 

Obs.  AOU  Pred  AO 

Speed  in  knots. 
Contact  Filtered 
mean  mean 

1 

0  407 

2.971 

3.138 

5 

1  077 

2.975 

2.919 

1 

0407 

9  996 

9549 

5 

1  077 

10.028 

9.011 

1 

0407 

29,997 

28.449 

5 

1.077 

30.011 

27.085 
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APPEP«)IX  G:  MODELS  OF  ACTUAL  ESTIMATED  FILTER  OPERATION 


This  file  simulates  the  basic  operation  of  the  ASSET  tracker.  A  contact  conducts  a  random  tour  at  constant  speed 
as  a  contact  would  in  the  simulation.  Time  interval  between  glimpses  is  an  exponentially  distributed  variable. 
Statistics  are  collected  to  see  how  well  the  tracker  tracks  the  target  over  the  course  of  its  path.  The  number  of 
points  plotted  was  adjusted  to  allow  details  to  be  seen. 

j  O..75  runiinw  48  glimpK  1  ln(md(  1  ))•  glimpse  mi  4  v  10  im^  0  u  10  mg  u 

uHj  ^  runtime  tm^  ,j  N2  lent  N )  I  t  *  0 . .  N2  n  0  . ,  N2  +1 

ai  ln(m<l(  1  ))  mia2  ln(  fttd(  1  ) )•  mi +iSl  A3  '  ln(  md(  1  )  )-mi  4A2  A4  =  lii(  md(  1  )  )•  mi  -HiS  A5  =  - ln(  m<J(  1  ) )•  mi 

A6  '  ln( Tidt  1  )  )•  mi  -rA5A7  ln(  md(  1  ) )•  mi  4A6A8  - ln(md(  I  ) )•  mi  +tl7A9  =  ■  ln(  md(  1  )  )•  mi  -rAScl  2'  T‘ nid(  1  )c2  2"  T* mdt  1  )  ^ 

cJ  2-TttkJ(I)c4  :2-T-md(l)c5  2-T-md(l)c6  2- X' r7>d(  1  )c?  2-T-n>d(l)c8  2-T-n>d(l)  c9  2-T-nid(l)  dO  :2-x-rad(l) 

'^n  ■'*  ““n  ““n  ’**'  •'=-  -‘f  •”'n  **"n  **”n  • 

randl^  ■  tTKl(  1  )  riid(  1  )  r*ndJ|j  ;  md(  1  )  r*nd4|^  ;  n>d(  1  ) 


«ngn 

•  2‘lrt  «ndl  • 

“  n 

CO*  2* 

X-r.od2n  ymgn 

fspdn  ^  V 

-Kmd(6  )  ■  3  )  ^*^0 

-  2'T' 

™4(  1  )  Yspdji 

0 

.  25  a  -  ,  ip 

Yspdn 

X.pd„  A„  4-Xd..n 

VMIfl  IC^ 

0 

♦3, 

,  exp  a-Ao 

Xspdn 

V  •  CO*  k' 

n 

2 

‘*‘n 

rmgn  4YdiSn 

:  .5  -  exp 

'"«n 

xmgn  +Xdis„ 

a 

yrspdn 

'«epd„ 

rspd^'sin  rc*e^ 

rspdn'^e  reie^ 

-1  2-ln  niuO^  -lui'  2-T-r»nd4^  rcse|j  mod  -f  md  ^  -  ,2-X 

•41fl  ’  tC_  XsDd.  ■  v’co*  u_  YdiVs  -  0  Xdii^  ■  0 


^  ^  rl  ^  ,25-u2  vIq  :  rl  v2o  ■  0  v3o  r3  i-  I  exp  a- 

<|1„  ^  An  1-  2-'  I  -  exp  -O- An  -  5-  1  exp  a- 2- Aq 
a  ■  ° 

2 

-f  5-exp  -a-2-An  q3„  ’  a  '  ****  a-2  An 


vl 


v3 


n  M 
n  M 
n  -H 


■  <*^0  ''’n  v3„  •  <>3„  4q2„  ^ 


‘=n  -^^n'vdn  ■  «3„  -N2„  '  rl  • 


''■n  •*^'n  ‘  '^^n  ‘•v3„  ■H,3„  -rr3  v2„ -rs*2n- v3n  •  «3„  it,2n  ^ 


^•v3n  -Ht3„  v2n  -H»2n ■  v3n  •  4>3n  -r<|2n  ^ 

'>0  “^^n  ^->3^ -rqln -iTl  •  t^Sn  -l-q3n  4t3  v2„-^2n-v3„  •4>3„^2^^ 


“'n  TT  71  TT  “^n  IT  ^  ^0  '"*0  '^O  '‘V) 


fnv2  fneta(v2) 
mv3  (ne4o(  v3  ) 
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Y,  a  4Vy,  Vy,-exp  O  il, 

“H  41 

Y,-a  4Vy,  Vy^-exp.  a-A, 

4k2.,  •  ytxpd,  4,  ^  Vy,-  txp,  -  a"  A, 

a 

a 

^  41 

X^*a-^Vx^  Vx^*exp  a*^ 

•  41- 

■»».  41- 

Vx^*exp. 

4k2x,  4|-  x«pd,  41  Vx,-exp  -a-A, 

\  41 

a 

a 

Vy,exp  a-A, 

4k2b,4i- 

‘*S41  ■ 

Y,-a  4Vy,  -  Vy,-exp  a-A, 

4X3,4,-  yrxpd,  4,  -  Vy,-exp  a-A, 

a 

Vx,-«xp’  a-A, 

‘"»t41  ■ 

'  Vyexfh  a 

4tJ,  4,  -  xrxpd,  4^  Vx,-exp'  a-A, 

4t2b,  4, 

a 

Vk)o2,  ^ 
V.ou3,  ^ 


vlj  +2-  02,-v2,  +  02,  -hil, 

v2,  -t^2,'v3,  -03,  -tti2, 

03,  ^ 


Parameters  for  this  run: 

D  =  metn  A  N2  =49  a  =7.07106* 


Xiou,  ^ 

''“*1  -H 
Vnoo,  ^ 
Vywu,  ^ 


X,  1  -«*P  a-\'  -v*, 

V,  -ti-  1  exp  a  \  •  Vyj 

exp  Of  A,  -Vx, 
exp  -a-\  -Vyj 


a  =0.25  u  =5 


AOU  -,ii»*n(vl)  lOU  =  ''““I )  AOU  =10*77**  100=1.197159 

Determine  the  difference  between  the  true  target  position,  and  the  filtered  position; 


Fx,  =  X,  Xd»,  Fy,  --  Y,  Yd.., 

the  true  target  position,  and  the  contact  position  and  the  true  target  position,  and  the  lOU-predicted  position: 

Cx,  :  In*,  Xdi.,  Cy,  =  l«<,  Y*.,  •*,  =  Xiai,  -  X*.,  ly,  =  Yk*.,  Ydit, 

The  root-mean-square  error  is  found  for  the  data: 

F2x,  =  •  Fx,  F2y,  =  '  Fy,  ^  Clx,  ^  '  Cx,  ay,  =  Cy,  Dx,  =  U,  ^  Oy,  =  ly, 


SFx 


:F2x  SFy  -  "Fly  SCx  ■  ICix  SCy  =  ZF2y 


Six  =  II2x  Sly  -  ZCy 


t  (SFx)  ^(SFy)  .  .  ^  [  (SCx)  ,  (SCy)  1.  ^  ^  '  (SU)  ^(Sly)  .  , 

1mi(Fx  )  -^1^  lui(Cx)  ;  .  tanTu  )  laii(ty) 

The  mean  filtered  and  contact  velocities  is  found: 


F*it  =0  952507 
Cm  =1  740496 


Im  =  I  01*29 


FV 


,  ;  :  Vx,  ^  4*  Vy,  ^  CV,  ^  Xf^!^ -Hynpd,!^  aiMB(FV)  =*.574437  mMnl  CV  )=  10  102023 

■ .  '  ‘  itiFV  -  mMn(FV) 

«dev(CV )  =1641369  mCV  niMnlCV) 


»(FV)  =0  773034 

The  RMS  error  in  filtered  position  is  compared  to  the  RMS  error  in  the 
contact  position: 


TR  Cm  -  Fm 


TR  =0  7*79*9 


The  difference  from  the  computed  error  is  then  computed  for 
the  current  run: 

COMPtrr  =  AOU  Fm  COMPm  =0  1352*1 
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Filtered  track  using  the  actual  algorithm,  with  mean  glimpse  interval  0.05  hour  and  Sensor  AOU  =  : 


At  =0  8‘>6I46 
A2  =i  :510J4 
A3  =3  104831 
ii4  =4 

A5  =6  68>:o» 

A6  =16  686341 
A7  =31  313004 
A8  =31  6115.'5 
A9  =34.i:2J9 


Xdu,  .lug, 


xeSn  “'n  *  2' ln( m<l(  1  ) ))•  co«  2 ■  T‘ rad(  1  )  vl , (  2"  ln( md(  1  ) ) )•  »iii  2 •  T* nid(  1  ) 

Vx,-exp  Ot-A,  -HJ,  ■  xruxi,  VXj-exp  a- A,  yVe,  Vy^-exp  a- A,  -HAj  •  ynpd,  ^  Vy^  •  exp  a-A, 
VdifFx,  xVe,  Vx,  m««n(Vdiffx)  =0  133326  Vdiffy,  yVe,  Vy,  me»n(  Vdiffy  )  =0.030425 


Vdiffy,  yVe,  Vy,  me»n(  Vdiffy  )  =0.030425 


Filtered  track  using  estimation  technique  (using  full  noise  values): 


Eax,  -  xe«j  -  Xdii, 
Eay,  yea,  Ydii, 


Filtered  track  using  the  actual  algorithm,  with  mean  glimpse  interval  0. 1  hours  and  Sensor  AOU  -  1 


At  =20«95<r’ 
^2  =10  470267 
^3  =14  93814 
M  =17  572047 
A5  =  20  489058 
A6  =22  232939 
A7  =  25,150709 
,^8  =  25  233842 
A9  =  45  284693 


■‘'"-15  -10  -5  0  5  10  15  20 

Xdu,  .log,  ,X, 

_  “*>0=0 

'“n  •  ^  2lii(n>d(l  )))co«  2-T-fnd(l  )  +Xdu„  yen^  =  ,  vT^- •,(- 2- ln(  nid(  1  )))•  tiOi  2' T- rnd(  1  )  -Hfdii^ 

xVe,  wyexf)  a- A,  Tt),  ^  •  xripd,  ^  Vx^-exp  -  a' A,  yVe,  --  Vy,-exp  -  a- A,  7*3,  ^  •  ynpd,  ^  -  Vy^-exp  O' A, 
VdifFx,  ■  xV«,  Vx,  me«n(VdiflFx)  =0.056692  Vdifly,  =  yVe,  -  Vy,  inein(  Vdifly )  =0.013961 


Vdifly,  =  yV«, "  Vy,  ineui(  Vdifly)  =0.013961 


Filtered  track  using  estimation  technique  (using  fuU  noise  values): 


Eju,  =  xe«,  -  Xdii, 
E«y,  =  yen,  Ydix, 

Ex,  =  E«x,  ^  Ey, 


Uni  Ex )  lan(  Ey  ) 
Eetr  =1.925021 
Ferr  =2.165015 

,  meant  vl  )  =2.072638 


0  5 

Xdu,  .log,  ,xc«, 


Filtered  track  using  the  actual  algorithm,  with  mean  glimpse  interval  0.5  hours  and  Sensor  AOU  - 


,^1  =9  179778 
^2  =W  746842 
a3  =  23  257026 
a4  =  23  336796 
as  =25  985588 
a6  =  26  130136 
A7  =  21  402486 
as  =28.369044 
=40  739811 


0  50  100  |  50  200  250  300 

.Xdu,  .log, 

__  _  *"*0^0 

xe»t^  vl ^-  (  2‘ ln{  tTid(  1  )  ))•  co»  2"  T" md(  1  )  r**n  '  ,  vl  , ( -  2‘ ln( rad(  1  )))•  iia  2‘ T‘ fnd(  1  )  T-YdiSu 

xVe,  Vx^-exp  a-A,  -Hdi  ^  •  xnpd,  ^  Vx,-«Jip' -a- A,  yVe,  Vy^-exp-a-A,  -HO,  ^  •  ynpd,  ^  Vy^- exp  O' i 

VdifFx,  xVe,  Vx,  me»n(Vdilfx)  =-0  104991  Vdi(^,  -  yVe,  Vy,  ii>ein(Vdifly )  =0  040473 


Filtered  track  using  estimation  technique  (using  full  noise  values): 


Eox^  =  xeMi '  Xd»| 

E«y,  ^  ye*S  •  Ydii, 


(SEx)  ^(SEy) 


.  lin(  Ex )  Ub(  Ey ) 
Eerr  =3  402076 


Filtered  track  using  the  actual  algorithm,  with  mean  glimpse  interval  0.5  hours  and  Sensor  AOU  —  - 


'*'!  -50} 


Xdn,  .log,  .X, 


ai  =3  91551: 
a2  =6  91615 
a3  =15  ’8^01 1 
M  =  :o  2oo:69 

as  =30,204378 
a6  =34  696944 
a7  =35  833538 
as  =38.376747 
a9  =38  569162 


«»o  ■  ° 


'“•n  . ''■o'  .(  1  )))-co«  2-r-md(l  )  +Xdis^  yen^  =  .  vl 2- ln( md(  1  )))•  «iQi  2- T- md(  1  )  -l-YdUi, 

»Ve,  Vx^-exp  a-a,  ^  •  xrxcx),  Vx^  cxp  a-a,  yVc^  Vy^- exp  a- a,  -lU,  ^  ■  yrgxl,  ^  Vy^- exp  or- a, 


Vdiffx,  xVe,  Vx,  mexn(  Vdiffx )  =-0.056681 


Vdiflfy,  -■  yVe,  -  Vy,  mean(Vdiffy)  =-0.145247 


Filtered  track  using  estimation  technique  (using  full  noise  values): 


E«u,  -  xe«,  -  Xdil, 

Extyj  '  ye»t|  -  YdiXj 


Filtered  track  using  the  actual  algorithm,  with  mean  glimpse  interval  1  hour  and  Sensor  AOU  - 


A1  =5  022727 

437775 
A3  =17  876472 
A4  =  28  662312 
A5  =  42.644194 
A6  =  47  842995 
A7  =49  239743 
A8  =50.274019 
A9  =53  491883 


-150  100 

Xdu,  .In*,  .X, 


'*“n  =  . ^  '*^‘*'*0  )))••>">  i’T'nidt  1  ) 

»Ve|  Vt,  «i(p  a-A,  4k3|  )tr»pd,  ^  Vxj-exp  a- A,  yVe,  =  Vy^  exp  a-A,  4t3,  ^  •  ymxl,  ^  Vy^  •  eip  -  a- A, 


Vdiffx,  •  xVe,  Vx, 


me»n(Vdiffx)  =0  472115 


Vdiffyj  :  yVe,  -  Vy^  meant  Vdif^  )  =-0.66914 


Filtered  track  using  estimation  technique  (using  full  noise  values): 


Eaix,  =  xeaCj  -  Xdia, 

E»«y,  =  y««S  ■  Ydii, 


’^<^■•,150 


y*“,  100 


(SEx)  ^(SEy) 


.  lait(Ex)  laii(Ey) 
Eerr  =4.03991 
FeiT  =3.395647 


Filtered  track  using  the  actual  algonthm,  with  mean  glimpse  interval  1  hour  and  Sensor  AOU  -  30nm: 


^1  =0  |5';067 


vln'  (  2- ln(  mil(  1  )))•  coi  ’■  T-rndt  1  ) 

xVe^  Vxj'exp  O' \  •  xnpdj  ^  •  Vita's xp  a- ^ 

VdiffXj  xVtj  VXj  me«n(Vdiffx)  =-0  495172 


vl^"-,(-2'ln(md(l  )))■  Jini  2‘f  md(l  )  -‘-VdU|j 
yVe,  Vy^-exp  -0-2\  TtJ,  ^  •  ynpd,  ^  Vy^ •  exp  a'ii, 
Vdifly,  --  yVe,  Vy,  me»n(VdiBy)  =0  0192* 


ye«0  ^  0 


Filtered  track  using  estimation  technique  (using  full  noise  values): 
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AOU  =  60nm: 


U  =110031”; 

12  =3  356652 

13  =11  ■»1)!0«4 

14  =14  422879 
^5  =19  24585'’ 
k6  =  24  564634 
il  =26.3703'’5 
k8  =30  333605 
i9  =30  9155'’’ 


N2  =54 


“O  •''‘“O  ^ 

id(  1  )  +Yd[S^ 


Vy^-cxp  aXi 
294623 


E«x, 

■  xeft^ 

Xd-. 

Eny, 

'  ycsij 

Ydii, 

•  Ey. 

E«y, 

SEx 

lEx 

SEy 

■  lEy 

Eerr 


(5Ex)  ^  (SEy) 

.  list  (Ex)  Itn(Ey) 
Eerr  =15  785448 


5 


Fen-  =12  430599 


■8 


8 


,  [iKxn(  V  1  )  =  1 5  165514 


APPENDIX  H:  KALMAN  GAIN  VERSUS  SENSOR  AOU  SIZE 


This  appendix  shows  the  relationship  between  the  steady  state  Kalman  gain  Ka  and  the  Sensor  AOU  size. 

The  impact  of  using  the  the  approximations  from  Appendix  A  is  also  illustrated.  The  data  for  each  case  was 
written  to  a  seperate  file  and  the  graph  alone  display^ 


n  0..2}  a  :  .1 

vli|j  rl  v2e,j  0  visy 


Vlr 


1000 
:  rl  v2, 


.25 


ul  -  2-1  exp  Of  a  5-  I  exp  OI‘  2-a 

2  Q  .  -  - 


a  -.50 


:0  0  vi(,  rJ 


a 


.25-u* 


*2  :  1  •  exp  tt-a  '  03  =  exp  a-a 

a 


q2  =  5  -  exp;  or-a ;  +  5-exp  -a-2-a  _ 


vl 


Q 


-H 


v3 


n  M 


q3  -  .5  -  —  *  I  -  exp’ 

a 


vl^  -t-2'02  v2j| -t>  02  -h]l  •  03  -h)3  4r3  -  v2|, -^02- v3||  •  03 -ttl2  ^ 

fl  __  :  _  ^  _ 

vl^  +-2-02-v2j,  02  ■v3^  -Kil  -frl  03  •v3|j  -h)3  -frJ  -  v2||  -03  -hj2 

v2u  ■t02-v3|j  -03  -tti2  -r!  -rS 

vl^ -^■2•02•v2^ -*•  02  ^•v3^ -hil  -iTl  •  03  ^-v3|, -ttjj -1x3  -  v2„ -i02-v3^  -03  ■t<i2  ^ 

vi^  •‘•2-02-v2|,  -ir  02  ^•v3^  -hjl  -1x1  •  03;^-v3„  -hi3  -  v2q  -t02-v3^  -03  -t<)2  ^ 

r3  -  ^  -  -  :  ■  ^  .  X  — —  ;  —  -= 

vl ^ -(-2- 02- v2^ -*02  •v3|j -K]!  -kl  •  03  -v3^ -1^3 -1x3  -  v2u -102- •  03 -H]2 


Using  the  transition  and  process  noise  approximations  from  Appendix  A  to  simplify  the  calculations: 


02£  :  a-  a-  a  •  2 


03c  ■  1  -4 -a- a-  a-a  -  2  1  qle  =  <7^-;  a  5  -  j-a-a 

2  ^-.34 


q2e  =  ^  ‘  •  Ofa; 


vie 

v2e 


n 

n  M 


vJe 


n  -H 


rl  ' 


vle^,  -*-2 - 02c - v2e||  -I*  02£j^-v3<||  -fqle  .-i  :  03c  '^•v3e„  -tqle  -*x3  vle^  +^2C'vie^  -03e  -fqZe 
'ICji  -*-2-02c-v2ejj  -I*  02c|^-v3ejj  -hjle  -kl  •  03c  l^-vSe^  -hj3c  -h-3  •  v2ejj  -*02c-v3e|,  *030  -IxiZe  ^ 


q3e  :  a  •  a-  1  -  a-  a  I 
2  : 


,  i"  v2*^  -r02£-v3e„  i-03c  -IqZ*  -fl  rl 


le^j  -*-2-02c-v2e|j  -♦*  02£J^-v3ej  -Iqle  -kl  ■-  ^  03C;^-v3eu  -♦q3e  -k3  -  v2e|j  -f02£-v3e|j  -03e  -K|2e  ^ 


vle^j  -l-2-02£.-v2ej|  -t*  02c  '^-viejj  -k|le  -kl 

-■  [  03e  ■ 

vZc^-h^ZC'vit^  -03e-tq2e  ^ 

vle^^  -*-2-02c-v2e||  -1*  02c  v3«^  -*<|le  -kl  - 

'  03c  ^'vJe|j  -k|3e  -k3 

v2e|j  -*02£-v3e^  -03c  -lq2e  ^ 

-r  k2a„ 
rl  n 


''2„ 

k2b„  ^  -P 

"  rl 


O  =  — ? 
"  t3 


kl«- 


''•'n 

fl 


v2c. 


k2ie. 


k2b«„ 


rl 


k3e. 


r3 
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